diff --git a/.reuse/dep5 b/.reuse/dep5 index 9de29b9c..e1345077 100644 --- a/.reuse/dep5 +++ b/.reuse/dep5 @@ -46,3 +46,7 @@ License: CC0-1.0 Files: doc/img/logo.png Copyright: 2023 Yaskawa America, Inc. License: CC-BY-NC-ND-4.0 + +Files: doc/img/RtFlow.png doc/img/RtFlow.vsdx +Copyright: 2023 Yaskawa America, Inc. +License: CC-BY-NC-ND-4.0 diff --git a/CHANGELOG.md b/CHANGELOG.md index 6cbf8db9..45c2b174 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,12 +1,20 @@ # Changelog +## Forthcoming + +MotoROS2 is now built against `micro_ros_motoplus` version TODO + +New functionality: + +- Add new motion mode for real-time control of the robot. This pipes the user commands directly to the motion API with minimal overhead. ([#449](https://github.com/Yaskawa-Global/motoros2/pull/449)) + ## 0.2.1 (2025-06-26) MotoROS2 is now built against `micro_ros_motoplus` version `20250328`. diff --git a/README.md b/README.md index 51b7870f..f8c5eaae 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ @@ -605,7 +605,12 @@ Instead, write a `FollowJointTrajectory` action *client* script or use a motion ### Commanding motion -The ROS API of MotoROS2 for commanding motion is similar to that of `motoman_driver` (with MotoROS1), and client applications are recommended to implement a similar flow of control to keep track of the state of the robot before, during and after trajectory and motion execution. +There are three methods of commanding motion using MotoROS2. +`FollowJointTrajectory` action server, point streaming, and real-time incremental control. + +#### - [FollowJointTrajectory](doc/ros_api.md#follow_joint_trajectory) action server + +The ROS API of MotoROS2 for commanding motion is similar to that of motoman_driver (with MotoROS1), and client applications are recommended to implement a similar flow of control to keep track of the state of the robot before, during and after trajectory and motion execution. The following provides a high-level overview of the behaviour a client application should implement to successfully interact with the [FollowJointTrajectory](doc/ros_api.md#follow_joint_trajectory) action server offered by MotoROS2. While not all steps are absolutely necessary, checking for errors and monitoring execution progress facilitates achieving robust execution and minimises unexpected behaviour in both client and server. @@ -632,7 +637,22 @@ As a final check, inspect the `error_code` field of the result to ascertain exec 1. if there are more trajectories to execute, return to step 2. Otherwise call the [stop_traj_mode](doc/ros_api.md#stop_traj_mode) service to exit MotoROS2's trajectory execution mode -Interaction with the *point streaming* interface (MotoROS2 `0.0.15` and newer) would be similar, although no `FollowJointTrajectory` action client would be created, no goals would be submitted and monitoring robot status would be done purely by subscribing to the [robot_status](doc/ros_api.md#robot_status) topic (instead of relying on an action client to report trajectory execution status). +#### - [QueueTrajectoryPoint](doc/ros_api.md#queue_traj_point) point streaming + +Interaction with the *point streaming* interface (MotoROS2 `0.0.15` and newer) would be similar to the process above, although no `FollowJointTrajectory` action client would be created, no goals would be submitted and monitoring robot status would be done purely by subscribing to the [robot_status](doc/ros_api.md#robot_status) topic (instead of relying on an action client to report trajectory execution status). + +Rather than submitting a complete trajectory in a single goal, an indefinite number of points are submitted to the robot one at a time. +The execution of the robot will be identical to the behavior of `FollowJointTrajectory`. + +#### - [StartRtMode](doc/ros_api.md#start_rt_mode) real-time incremental motion + +This activates a separate UDP server which listens for real-time position increments. +It is intended to be used in a closed loop system which requires low level control of the motion. +This UDP server is activated using a ROS2 service. +However, the protocol for commanding/monitoring motion is *not* based on ROS2 communication. + +This control mode minimizes overhead as much as possible by routing the user commands directly to the motion API. +See [R/T Motion Control](doc/rt_control.md) for information on the protocol implementation. ### With MoveIt @@ -844,9 +864,7 @@ The following items are on the MotoROS2 roadmap, and are listed here in no parti - native (ie: Agent-less) communication - support asynchronous motion / partial goals - complete ROS parameter server support (there is currently no support for `string`s in RCL) -- real-time position streaming interface (skipping MotoROS2's internal motion queue) - Cartesian motion interfaces -- velocity control (based on `mpExRcsIncrementMove(..)`) - integration with ROS logging (`rosout`) - publishing static transforms to `tf_static` - integrate a UI into the teach pendant / Smart Pendant diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index d726861b..03a8d9e3 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -318,3 +318,50 @@ publisher_qos: # OPTIONS: USER_LAN1, USER_LAN2 # DEFAULT: (all available network ports) #debug_broadcast_port: USER_LAN1 + +#----------------------------------------------------------------------------- +# For the real time motion interface, which port should the UDP messages +# be transmitted on? +# +# DEFAULT: 22000 +#rt_listener_udp_port_number: 22000 + +#----------------------------------------------------------------------------- +# For the real time robot status interface, which port should the UDP messages +# be transmitted on? +# +# DEFAULT: 22001 +#rt_status_udp_port_number: 22001 + +#----------------------------------------------------------------------------- +# For the real time robot status interface, how much delay should there be +# between messages? +# +# DEFAULT: 10 +#rt_status_sleep_period: 10 + +#----------------------------------------------------------------------------- +# Timeout for real time motion commands. If a command packet is not received +# within this number of milliseconds, the motion mode will be cancelled. This +# applies to all packets within R/T session. +# +# Additionally, if the client does not receive a reply packet within this +# amount of time, then it should be assumed that the session is dead. +# +# Setting this to '-1' will never timeout. In that case, you must explicitly +# call '/stop_traj_mode' to stop the motion mode. +# +# DEFAULT: 5000 (5.000 seconds) +#timeout_for_rt_msg: 5000 + +#----------------------------------------------------------------------------- +# When using the real time motion interface, each command packet must increment +# the sequence ID. If too many packets are lost during communication, then it +# will be assumed that the PC is not in sync with the robot. +# +# If the sequence ID of an incoming packet is different from the previous +# command by a value greater than this, then the connection will be dropped. +# The motion mode must be reactivated to be used again. +# +# DEFAULT: 3 +#max_sequence_diff_for_rt_msg: 3 diff --git a/doc/img/RtFlow.png b/doc/img/RtFlow.png new file mode 100644 index 00000000..89d3f393 Binary files /dev/null and b/doc/img/RtFlow.png differ diff --git a/doc/img/RtFlow.vsdx b/doc/img/RtFlow.vsdx new file mode 100644 index 00000000..79a7a56c Binary files /dev/null and b/doc/img/RtFlow.vsdx differ diff --git a/doc/ros_api.md b/doc/ros_api.md index 0f4950b7..bb7a0988 100644 --- a/doc/ros_api.md +++ b/doc/ros_api.md @@ -1,6 +1,6 @@ @@ -133,6 +133,21 @@ Check the relevant fields of the `RobotStatus` messages to determine overall con The `reset_error` service can be used to attempt to reset errors and alarms +### start_rt_mode + +Type: [motoros2_interfaces/srv/StartRtMode](https://github.com/yaskawa-global/motoros2_interfaces/srv/StartRtMode.srv) + +Attempts to enable servo drives, activate the [real-time UDP server](rt_control.md), and set the job-cycle mode to allow execution of `INIT_ROS`. +This allows the user to send incremental motion to the robot at the rate returned by the service (`period`). + +See [R/T Motion Control](rt_control.md) for information on the protocol implementation. + +Note: this service may fail if controller state prevents it from transitioning to R/T mode. +Inspect the `result_code` field to determine the cause. +Check the relevant fields of the `RobotStatus` messages to determine overall controller status. + +The `reset_error` service can be used to attempt to reset errors and alarms + ### stop_traj_mode Type: [std_srvs/srv/Trigger](https://github.com/ros2/common_interfaces/blob/37ebe90cbfa91bcdaf69d6ed39c08859c4c3bcd4/std_srvs/srv/Trigger.srv) diff --git a/doc/rt_control.md b/doc/rt_control.md new file mode 100644 index 00000000..369b6cb0 --- /dev/null +++ b/doc/rt_control.md @@ -0,0 +1,219 @@ + + +# R/T Motion Control + +The real-time motion control server is intended to be used in a closed loop system. +It allows the user to command incremental offsets at the rate of the robot controller's interpolation clock. +This control mode minimizes overhead as much as possible by routing the user commands directly to the MotoPlus motion API, mpExRcsIncrementMove. + +## Activation + +This control mode is activated using the [start_rt_mode](ros_api.md#start_rt_mode) service. +The user must specify the `control_mode` to indicate whether the increments will be joint offsets (radians) or cartesian TCP offsets (meters / quaternion). + +If this service is successful, it will return a `result_code` of `Ready (1)`. +Otherwise, please examine the `result_code` and `message` files in the response for more information. + +The service will also return a `period` in milliseconds. +This indicates the rate at which increment commands will be expected by the robot. +The default period for a single manipulator is 4 milliseconds. +However, that value will increase as additional axes or manipulators are added to the system. + +## Usage + +### Command Flow + +Once activated, a UDP server will listen on port `22000` (default). +The user then sends the first increment with a the `sequenceId` field set to `0`. +After that, the user must wait until the robot replies before sending the next increment. +Each subsequent command must increment the `sequenceId`. Additionally, each subsequent command must not be sent until the robot replies to the previous command. +This will occur at the rate of the `period` from the [start_rt_mode](ros_api.md#start_rt_mode) service. + +If a command is not received with 5 seconds (default), then the session times out and is dropped. +At that point, the server must be reactivated by calling `stop_traj_mode` and `start_rt_mode`. +A "keep-alive" can be used by sending a command with zero increments. + +Additionally, if the client does not receive a reply packet within this amount of time, then it should be assumed that the session is dead. + +Command Flow + +### Data format (command) + +The command packet is a *packed* `RtPacket` structure. + +```c +//########################################################################## +// !All data is little-endian! +//########################################################################## +struct RtPacket +{ + //The version of the command packet must match the value expected + //by MotoROS2. + + int version; + + //The packet type must match the control_mode which was specified + //in when invoking the start_rt_mode service. + + PacketType packetType; + + //Must increment sequentially with each new command packet. + + UINT32 sequenceId; + + //The order of the joints must be in the order of [S L U R B T E 8]. + //Please note that for seven axis robots, the 'E' joint is phyically + //mounted in the middle of the arm. But it must be sent at the end + //of the joint array. See JointIndices enum. + // + //For joint-space, this will be radians of each joint. + // + //For cartesian, this will be meters and quaternion of the TCP. + //The order of the joints must be in the order of [X Y Z Qx Qy Qz Qw Re]. + //See CartesianIndices enum. + + double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //Set tool that will be used by motion API (ie: passed by us to mpExRcsIncrementMove(..)) + //NOTE: this will change the 'motion tool' ONLY for those increments which + // haven't yet been added to the increment queue. See also the ROS 2 + // 'select_tool' service definition file in motoros2_interfaces. + + int toolIndex[MAX_GROUPS]; //TOOL 0 - 63 + + //Reserved for future expansion + + char reserved[64]; +} +``` + +The `version` must match the version number expected by the server. +If it does not match the expected value, the packet will be rejected and the connection will be dropped. +The current version is `1`. + +#### Joints + +When the `control_mode` is `JOINT_ANGLES (1)`, the order of the joints in the `delta` array must be in the order of `S L U R B T E 8`. +Please note that for seven axis robots, the `E` joint is phyically mounted in the middle of the arm. +But it must be sent at the end of the joint array. + +See `JointIndices` enum. + +```c +enum JointIndices +{ + Joint_S = 0, //radians + Joint_L, + Joint_U, + Joint_R, + Joint_B, + Joint_T, + Joint_E, + Joint_8, + + MAX_JOINTS +} +``` + +#### Cartesian + +When the `control_mode` is `CARTESIAN (2)`, the order of the joints in the `delta` array must be in order of `X Y Z Qx Qy Qz Qw Re`. + +See `CartesianIndices` enum. + +```c +enum CartesianIndices +{ + TCP_X = 0, //meters + TCP_Y, + TCP_Z, + + TCP_Qx, //quaternion + TCP_Qy, + TCP_Qz, + TCP_Qw, + + TCP_Re, //radians + + MAX_AXES +} +``` + +### Data format (reply) + +The command packet is a *packed* `RtReply` structure. +This will echo the sequence ID, provide feedback position, and provide commanded position. + +Additionally, there is a flag to indicate if the Functional Safety Unit (FSU) reduced the speed of the **previous** command cycle. +This indicates that the robot did not complete the full increment as commanded. + +```c +//########################################################################## +// !All data is little-endian! +//########################################################################## +struct RtReply +{ + UINT32 sequenceEcho; + + //This is indicative of where the robot is physically located. + //Please note that this will trail behind the commanded position. + //The joint ordering will match that of the original command + //packet. See JointIndices and CartesianIndices enums. + + double feedbackPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; + double feedbackPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //The command position is the target destination you are instructing + //the robot to reach. It's the calculated endpoint based on the sum + //of all position increments received from the user. + // + //This is used to track if the robot's speed is being limited + //by the Functional Safety Unit (FSU). It can also be used to + //monitor the latency between command and feedback. + + double previousCommandPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; + double previousCommandPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //If the FSU speed limit is enabled, it can truncate the commanded + //delta increments. This flag is an indicator that the *previous* + //command cycle was truncated. It does NOT indicate that this most + //recent command packet was truncated. + + bool fsuInterferenceDetected; +} +``` + +## Deactivation + +Other motion modes may not be used at the same time as the real-time motion control server. +The service to start those modes will fail when invoked. +By calling `stop_traj_mode`, the R/T server will be disposed. +At that time, another motion mode may be used. + +## R/T Status Monitoring + +When the R/T Motion Control is activated, MotoROS2 will begin to send the `RobotState` structure on port UDP `22001` (default). +This is essentially a clone of the `/robot_status topic`. +But decoupled from the `industrial_msgs/RobotStatus` type. + +```c +struct RobotState +{ + int version; + + BOOL drives_powered; + BOOL e_stopped; + BOOL in_motion; + BOOL play_mode; + BOOL motion_possible; + BOOL error; + int error_code; +} +``` + +This will be sent every `rt_status_sleep_period` milliseconds. diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md index e2506cde..f297ae73 100644 --- a/doc/troubleshooting.md +++ b/doc/troubleshooting.md @@ -582,7 +582,7 @@ The name must not be blank. After correcting the configuration, the [changes will need to be propagated to the Yaskawa controller](../README.md#updating-the-configuration). -### Alarm: 8011[23 - 54] +### Alarm: 8011[23 - 54] or [56 - 58] or [66 - 67] *Example:* @@ -611,7 +611,7 @@ ALARM 8011 [xx] ``` -Where `[xx]` is a subcode in the ranges `[23 - 54]` or `[56 - 58]`. +Where `[xx]` is a subcode in the ranges `[23 - 54]` or `[56 - 58]` or `[66 - 67]`. *Solution:* These alarms are often caused by version incompatibilities between ROS 2 (on the client PC), micro-ROS (as part of MotoROS2) and/or the micro-ROS Agent. @@ -662,7 +662,7 @@ After correcting the configuration, the [changes will need to be propagated to t ### Alarm: 8011[56 - 58] -Please refer to [Alarm: 8011[23 - 54]](#alarm-801123---54). +Please refer to [Alarm: 8011[23 - 54]](#alarm-801123---54-or-56---58-or-66---67). ### Alarm: 8011[59] @@ -757,6 +757,81 @@ Describe the problem and include the following items: - copy of `motoros2_config.yaml` copied from the robot controller. - verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). +### Alarm: 8011[66] + +*Example:* + +```text +ALARM 8011 + Failed to init service (x) +[66] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + +### Alarm: 8011[67] + +*Example:* + +```text +ALARM 8011 + Failed adding service (x) +[67] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + +### Alarm: 8011[68] + +*Example:* + +```text +ALARM 8011 + Failed to allocate RT socket +[68] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + +### Alarm: 8011[69] + +*Example:* + +```text +ALARM 8011 + Failed to bind RT socket +[69] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + +### Alarm: 8011[70] + +*Example:* + +```text +ALARM 8011 + Failed to allocate RT socket +[70] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + ### Alarm: 8012[xx] *Example:* diff --git a/src/ActionServer_FJT.c b/src/ActionServer_FJT.c index f74de5f7..f9b08064 100644 --- a/src/ActionServer_FJT.c +++ b/src/ActionServer_FJT.c @@ -346,7 +346,7 @@ void Ros_ActionServer_FJT_ResetProgressTracker() //TODO: do multidof too } -//Called from TrajectoryMotionControl::Ros_MotionControl_IncMoveLoopStart +//Called from TrajectoryMotionControl::Ros_MotionControl_NonRtIncMoveLoopStart void Ros_ActionServer_FJT_UpdateProgressTracker(MP_EXPOS_DATA* incrementData) { if (fjt_active_goal_handle == NULL) diff --git a/src/CommunicationExecutor.c b/src/CommunicationExecutor.c index 25a358d5..7b63d22b 100644 --- a/src/CommunicationExecutor.c +++ b/src/CommunicationExecutor.c @@ -381,6 +381,11 @@ void Ros_Communication_StartExecutors(SEM_ID semCommunicationExecutorStatus) g_messages_QueueTrajPoint.response, Ros_ServiceQueueTrajPoint_Trigger); motoRos_RCLAssertOK_withMsg(rc, SUBCODE_FAIL_ADD_SERVICE_QUEUE_POINT, "Failed adding service (%d)", (int)rc); + rc = rclc_executor_add_service( + &executor_motion_control, &g_serviceStartRtMode, &g_messages_StartRtMode.request, + &g_messages_StartRtMode.response, Ros_ServiceStartRtMode_Trigger); + motoRos_RCLAssertOK_withMsg(rc, SUBCODE_FAIL_ADD_SERVICE_START_RT_MODE, "Failed adding service (%d)", (int)rc); + rc = rclc_executor_add_service( &executor_motion_control, &g_serviceSelectMotionTool, &g_messages_SelectMotionTool.request, &g_messages_SelectMotionTool.response, Ros_ServiceSelectMotionTool_Trigger); diff --git a/src/CommunicationExecutor.h b/src/CommunicationExecutor.h index f4112fa0..bb05b763 100644 --- a/src/CommunicationExecutor.h +++ b/src/CommunicationExecutor.h @@ -18,10 +18,11 @@ // service reset 1 // service start_traj_mode 1 // service start_point_queue_mode 1 +// service start_rt_mode 1 // service stop_traj_mode 1 // service queue_traj_point 1 // service select_tool 1 -#define QUANTITY_OF_HANDLES_FOR_MOTION_EXECUTOR (9) +#define QUANTITY_OF_HANDLES_FOR_MOTION_EXECUTOR (10) // total number of handles = // timers + 1 diff --git a/src/ConfigFile.c b/src/ConfigFile.c index b8d09de0..415ffe71 100644 --- a/src/ConfigFile.c +++ b/src/ConfigFile.c @@ -123,6 +123,11 @@ Configuration_Item Ros_ConfigFile_Items[] = { "ignore_missing_calib_data", &g_nodeConfigSettings.ignore_missing_calib_data, Value_Bool }, { "debug_broadcast_enabled", &g_nodeConfigSettings.debug_broadcast_enabled, Value_Bool }, { "debug_broadcast_port", &g_nodeConfigSettings.debug_broadcast_port, Value_UserLanPort }, + { "rt_listener_udp_port_number", g_nodeConfigSettings.rt_listener_udp_port_number, Value_String }, + { "rt_status_udp_port_number", g_nodeConfigSettings.rt_status_udp_port_number, Value_String }, + { "rt_status_sleep_period", &g_nodeConfigSettings.rt_status_sleep_period, Value_Int }, + { "timeout_for_rt_msg", &g_nodeConfigSettings.timeout_for_rt_msg, Value_Int }, + { "max_sequence_diff_for_rt_msg", &g_nodeConfigSettings.max_sequence_diff_for_rt_msg, Value_Int }, }; void Ros_ConfigFile_SetAllDefaultValues() @@ -214,15 +219,38 @@ void Ros_ConfigFile_SetAllDefaultValues() //inform_job_name snprintf(g_nodeConfigSettings.inform_job_name, MAX_JOB_NAME_LEN, "%s", DEFAULT_INFORM_JOB_NAME); + //========= //allow_custom_inform g_nodeConfigSettings.allow_custom_inform_job = DEFAULT_ALLOW_CUSTOM_INFORM; + //========= //userlan monitoring g_nodeConfigSettings.userlan_monitor_enabled = DEFAULT_ULAN_MON_ENABLED; g_nodeConfigSettings.userlan_monitor_port = DEFAULT_ULAN_MON_LINK; + //========= //ignore_missing_calib_data g_nodeConfigSettings.ignore_missing_calib_data = DEFAULT_IGNORE_MISSING_CALIB; + + //========= + //rt_listener_udp_port_number + sprintf(g_nodeConfigSettings.rt_listener_udp_port_number, "%s", DEFAULT_RT_LISTENER_UDP_PORT_NUMBER); + + //========= + //rt_status_udp_port_number + sprintf(g_nodeConfigSettings.rt_status_udp_port_number, "%s", DEFAULT_RT_STATUS_UDP_PORT_NUMBER); + + //========= + //rt_status_sleep_period + g_nodeConfigSettings.rt_status_sleep_period = DEFAULT_RT_STATUS_SLEEP_PERIOD; + + //========= + //timeout_for_rt_msg + g_nodeConfigSettings.timeout_for_rt_msg = DEFAULT_TIMEOUT_FOR_RT_MSG; + + //========= + //max_sequence_diff_for_rt_msg + g_nodeConfigSettings.max_sequence_diff_for_rt_msg = DEFAULT_MAX_SEQUENCE_DIFFERENCE; } void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event) @@ -743,6 +771,11 @@ void Ros_ConfigFile_PrintActiveConfiguration(Ros_Configuration_Settings const* c Ros_Debug_BroadcastMsg("Config: ignore_missing_calib_data = %d", config->ignore_missing_calib_data); Ros_Debug_BroadcastMsg("Config: debug_broadcast_enabled = %d", config->debug_broadcast_enabled); Ros_Debug_BroadcastMsg("Config: debug_broadcast_port = %d", config->debug_broadcast_port); + Ros_Debug_BroadcastMsg("Config: rt_listener_udp_port_number = %s", config->rt_listener_udp_port_number); + Ros_Debug_BroadcastMsg("Config: rt_status_udp_port_number = %s", config->rt_status_udp_port_number); + Ros_Debug_BroadcastMsg("Config: rt_status_sleep_period = %d", config->rt_status_sleep_period); + Ros_Debug_BroadcastMsg("Config: timeout_for_rt_msg = %d", config->timeout_for_rt_msg); + Ros_Debug_BroadcastMsg("Config: max_sequence_diff_for_rt_msg = %d", config->max_sequence_diff_for_rt_msg); } void Ros_ConfigFile_Parse() diff --git a/src/ConfigFile.h b/src/ConfigFile.h index 878d8f4d..c3dcfa0b 100644 --- a/src/ConfigFile.h +++ b/src/ConfigFile.h @@ -109,6 +109,15 @@ typedef enum #else #define DEFAULT_ULAN_DEBUG_BROADCAST_PORT CFG_ROS_USER_LAN1 #endif + +#define DEFAULT_RT_LISTENER_UDP_PORT_NUMBER "22000" +#define DEFAULT_RT_STATUS_UDP_PORT_NUMBER "22001" +#define DEFAULT_RT_STATUS_SLEEP_PERIOD 10 + +#define DEFAULT_TIMEOUT_FOR_RT_MSG 5000 + +#define DEFAULT_MAX_SEQUENCE_DIFFERENCE 3 + typedef struct { //TODO(gavanderhoorn): add support for unsigned types @@ -154,6 +163,13 @@ typedef struct BOOL debug_broadcast_enabled; Ros_UserLan_Port_Setting debug_broadcast_port; + + char rt_listener_udp_port_number[MAX_YAML_STRING_LEN]; + char rt_status_udp_port_number[MAX_YAML_STRING_LEN]; + int rt_status_sleep_period; + + int timeout_for_rt_msg; + int max_sequence_diff_for_rt_msg; } Ros_Configuration_Settings; extern Ros_Configuration_Settings g_nodeConfigSettings; diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c index 757b8b40..5b71fea6 100644 --- a/src/ControllerStatusIO.c +++ b/src/ControllerStatusIO.c @@ -168,27 +168,10 @@ BOOL Ros_Controller_Initialize() //================================== //create message for robot status //TODO(gavanderhoorn): use micro_ros_utilities_create_message_memory(..) instead - g_messages_RobotStatus.msgRobotStatus = industrial_msgs__msg__RobotStatus__create(); - rosidl_runtime_c__int32__Sequence__init(&g_messages_RobotStatus.msgRobotStatus->error_codes, MAX_ALARM_COUNT + 1); - - //================================== - // If not started, start the IncMoveTask (there should be only one instance of this thread) - if (g_Ros_Controller.tidIncMoveThread == INVALID_TASK) + if (g_messages_RobotStatus.msgRobotStatus == NULL) //may already be allocated in RealTimeMotionControl.c { - Ros_Debug_BroadcastMsg("Creating new task: IncMoveTask"); - - g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, - (FUNCPTR)Ros_MotionControl_IncMoveLoopStart, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); - if (g_Ros_Controller.tidIncMoveThread == ERROR) - { - Ros_Debug_BroadcastMsg("Failed to create task for incremental-motion. Check robot parameters."); - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; - Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); - mpSetAlarm(ALARM_TASK_CREATE_FAIL, APPLICATION_NAME " FAILED TO CREATE TASK", SUBCODE_INCREMENTAL_MOTION); - - return FALSE; - } + g_messages_RobotStatus.msgRobotStatus = industrial_msgs__msg__RobotStatus__create(); + rosidl_runtime_c__int32__Sequence__init(&g_messages_RobotStatus.msgRobotStatus->error_codes, MAX_ALARM_COUNT + 1); } //================================== @@ -247,8 +230,11 @@ void Ros_Controller_Cleanup() } } - mpDeleteTask(g_Ros_Controller.tidIncMoveThread); - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + if (g_Ros_Controller.tidIncMoveThread != INVALID_TASK) + { + mpDeleteTask(g_Ros_Controller.tidIncMoveThread); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + } Ros_Debug_BroadcastMsg("Cleanup publisher robot status"); ret = rcl_publisher_fini(&g_publishers_RobotStatus.robotStatus, &g_microRosNodeInfo.node); @@ -256,6 +242,7 @@ void Ros_Controller_Cleanup() Ros_Debug_BroadcastMsg("Failed cleaning up robot status publisher: %d", ret); industrial_msgs__msg__RobotStatus__destroy(g_messages_RobotStatus.msgRobotStatus); + g_messages_RobotStatus.msgRobotStatus = NULL; MOTOROS2_MEM_TRACE_REPORT(ctrlr_fini); } diff --git a/src/CtrlGroup.h b/src/CtrlGroup.h index 953b9e83..29a2d469 100644 --- a/src/CtrlGroup.h +++ b/src/CtrlGroup.h @@ -128,6 +128,7 @@ extern void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long const pulse extern void Ros_CtrlGroup_ConvertToRosTorque(CtrlGroup* ctrlGroup, double const motoTorque[MAX_PULSE_AXES], double rosTorque[MAX_PULSE_AXES]); extern void Ros_CtrlGroup_ConvertToMotoPos_FromSequentialOrdering(CtrlGroup* ctrlGroup, double const radPos[MAX_PULSE_AXES], long pulsePos[MAX_PULSE_AXES]); extern void Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(CtrlGroup* ctrlGroup, double const rosPos[MAX_PULSE_AXES], long motopulsePos[MAX_PULSE_AXES]); +extern void Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(CtrlGroup* ctrlGroup, long const motopulsePos[MAX_PULSE_AXES], double rosPos[MAX_PULSE_AXES]); extern UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup); diff --git a/src/ErrorHandling.c b/src/ErrorHandling.c index 7852e64b..af79088e 100644 --- a/src/ErrorHandling.c +++ b/src/ErrorHandling.c @@ -78,6 +78,8 @@ const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCo return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_ECO_MODE_STR; case MOTION_NOT_READY_SERVO_ON_TIMEOUT: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_SERVO_ON_TIMEOUT_STR; + case MOTION_NOT_READY_INVALID_SELECTION: + return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_INVALID_SELECTION_STR; default: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_UNSPECIFIED_STR; } diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index e36a44f2..88a0b425 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -35,6 +35,7 @@ typedef enum MOTION_NOT_READY_MAJOR_ALARM = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_MAJOR_ALARM, MOTION_NOT_READY_ECO_MODE = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_ECO_MODE, MOTION_NOT_READY_SERVO_ON_TIMEOUT = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_SERVO_ON_TIMEOUT, + MOTION_NOT_READY_INVALID_SELECTION = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_INVALID_SELECTION, } MotionNotReadyCode; typedef enum @@ -176,6 +177,12 @@ typedef enum SUBCODE_FAIL_INVALID_BASE_TRACK_MOTION_TYPE, SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA, SUBCODE_CONFIGURATION_FILE_YAML_PARSING_ERROR, + SUBCODE_FAIL_INIT_SERVICE_START_RT_MODE, + SUBCODE_FAIL_ADD_SERVICE_START_RT_MODE, + SUBCODE_FAIL_ALLOCATE_RT_CMD_SOCKET, + SUBCODE_FAIL_BIND_RT_SOCKET, + + SUBCODE_FAIL_ALLOCATE_RT_FB_SOCKET } ALARM_ASSERTION_FAIL_SUBCODE; //8011 diff --git a/src/MathConstants.h b/src/MathConstants.h index 62f5e76f..f9b57a48 100644 --- a/src/MathConstants.h +++ b/src/MathConstants.h @@ -16,6 +16,7 @@ #define DEGREES_PER_RAD (57.295779513082) // macro +#define METERS_TO_MILLIMETERS(x) (x * 1000) #define MICROMETERS_TO_METERS(x) (x * 0.000001) #define METERS_TO_MICROMETERS(x) (x * 1000000) #define RAD_TO_DEG_0001(x) (x * DEGREES_PER_RAD * 10000) diff --git a/src/MotionControl.c b/src/MotionControl.c index 428bdfda..8ee575c8 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -669,7 +669,7 @@ UINT16 Ros_MotionControl_ProcessQueuedTrajectoryPoint(motoros2_interfaces__srv__ //------------------------------------------------------------------- // Task to move the robot at each interpolation increment //------------------------------------------------------------------- -void Ros_MotionControl_IncMoveLoopStart() //<-- IP_CLK priority task +void Ros_MotionControl_NonRtIncMoveLoopStart() //<-- IP_CLK priority task { MP_EXPOS_DATA moveData; @@ -1328,6 +1328,46 @@ static STATUS Ros_Controller_DisableEcoMode() return NG; } +BOOL StartInterpolationTask(MOTION_MODE mode) +{ + //================================== + // If not started, start the IncMoveTask (there should be only one instance of this thread) + if (g_Ros_Controller.tidIncMoveThread == INVALID_TASK) + { + Ros_Debug_BroadcastMsg("Creating new task: IncMoveTask"); + + if (mode == MOTION_MODE_TRAJECTORY || mode == MOTION_MODE_POINTQUEUE) + { + g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, + (FUNCPTR)Ros_MotionControl_NonRtIncMoveLoopStart, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + } + else if (mode == MOTION_MODE_RT_JOINT || mode == MOTION_MODE_RT_CARTESIAN) + { + g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, + (FUNCPTR)Ros_RtMotionControl_HyperRobotCommanderX5, + (int)mode, 0, 0, 0, 0, 0, 0, 0, 0, 0); + } + else + return FALSE; + + if (g_Ros_Controller.tidIncMoveThread == ERROR) + { + Ros_Debug_BroadcastMsg("Failed to create task for incremental-motion. Check robot parameters."); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); + mpSetAlarm(ALARM_TASK_CREATE_FAIL, APPLICATION_NAME " FAILED TO CREATE TASK", SUBCODE_INCREMENTAL_MOTION); + + return FALSE; + } + + return TRUE; + } + + Ros_Debug_BroadcastMsg("ERROR - IncMoveTask is already allocated"); + return FALSE; +} + //----------------------------------------------------------------------- // Attempts to start playback of a job to put the controller in RosMotion mode // @@ -1355,7 +1395,7 @@ MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_ru Ros_Controller_IoStatusUpdate(); // Check if already in the proper mode - if (Ros_Controller_IsMotionReady()) + if (Ros_Controller_IsMotionReady() && Ros_MotionControl_ActiveMotionMode != MOTION_MODE_INACTIVE) { Ros_Debug_BroadcastMsg("Already active"); return MOTION_READY; @@ -1512,6 +1552,9 @@ MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_ru } } + if (!StartInterpolationTask(mode)) + return MOTION_NOT_READY_ERROR; + // have to initialize the prevPulsePos that will be used when interpolating the traj for(grpNo = 0; grpNo < g_Ros_Controller.numGroup; ++grpNo) { @@ -1582,6 +1625,9 @@ MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_ru void Ros_MotionControl_StopTrajMode() { + if (Ros_MotionControl_IsMotionMode_RealTime()) + Ros_RtMotionControl_Cleanup(); + Ros_MotionControl_AllGroupsInitComplete = FALSE; Ros_MotionControl_ActiveMotionMode = MOTION_MODE_INACTIVE; @@ -1594,6 +1640,12 @@ void Ros_MotionControl_StopTrajMode() ioWriteData.ulAddr = g_Ros_Controller.ioStatusAddr[IO_ROBOTSTATUS_WAITING_ROS].ulAddr; ioWriteData.ulValue = 0; mpWriteIO(&ioWriteData, 1); + + if (g_Ros_Controller.tidIncMoveThread != INVALID_TASK) + { + mpDeleteTask(g_Ros_Controller.tidIncMoveThread); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + } } BOOL Ros_MotionControl_IsMotionMode_Trajectory() @@ -1608,14 +1660,10 @@ BOOL Ros_MotionControl_IsMotionMode_PointQueue() MOTION_MODE_POINTQUEUE); } -BOOL Ros_MotionControl_IsMotionMode_RawStreaming() +BOOL Ros_MotionControl_IsMotionMode_RealTime() { - return FALSE; - - //TODO - // - //return (Ros_MotionControl_ActiveMotionMode == - // STREAMING_RAW_INCREMENTS); + return (Ros_MotionControl_ActiveMotionMode == MOTION_MODE_RT_JOINT || + Ros_MotionControl_ActiveMotionMode == MOTION_MODE_RT_CARTESIAN); } void Ros_MotionControl_ValidateMotionModeIsOk() diff --git a/src/MotionControl.h b/src/MotionControl.h index 14301ee3..e7dbebae 100644 --- a/src/MotionControl.h +++ b/src/MotionControl.h @@ -19,11 +19,13 @@ typedef enum { MOTION_MODE_INACTIVE, MOTION_MODE_TRAJECTORY, - MOTION_MODE_POINTQUEUE + MOTION_MODE_POINTQUEUE, + MOTION_MODE_RT_JOINT, + MOTION_MODE_RT_CARTESIAN, } MOTION_MODE; extern Init_Trajectory_Status Ros_MotionControl_InitTrajectory(control_msgs__action__FollowJointTrajectory_SendGoal_Request* pending_ros_goal_request); -extern void Ros_MotionControl_IncMoveLoopStart(); +extern void Ros_MotionControl_NonRtIncMoveLoopStart(); extern void Ros_MotionControl_AddToIncQueueProcess(CtrlGroup* ctrlGroup); extern UINT16 Ros_MotionControl_ProcessQueuedTrajectoryPoint(motoros2_interfaces__srv__QueueTrajPoint_Request* request); extern BOOL Ros_MotionControl_AddPulseIncPointToQ(CtrlGroup* ctrlGroup, Incremental_data const* dataToEnQ); @@ -38,7 +40,7 @@ extern void Ros_MotionControl_StopTrajMode(); extern BOOL Ros_MotionControl_IsMotionMode_Trajectory(); extern BOOL Ros_MotionControl_IsMotionMode_PointQueue(); -extern BOOL Ros_MotionControl_IsMotionMode_RawStreaming(); +extern BOOL Ros_MotionControl_IsMotionMode_RealTime(); extern void Ros_MotionControl_ValidateMotionModeIsOk(); diff --git a/src/MotoROS.h b/src/MotoROS.h index 85f4d18f..ae48aa93 100644 --- a/src/MotoROS.h +++ b/src/MotoROS.h @@ -9,7 +9,7 @@ #define MOTOROS2_MOTOROS_H #define APPLICATION_NAME "MotoROS2" -#define APPLICATION_VERSION "0.2.1" +#define APPLICATION_VERSION "0.2.2-pre" #include "motoPlus.h" @@ -71,9 +71,11 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -100,9 +102,11 @@ #include "ServiceResetError.h" #include "ServiceStartTrajMode.h" #include "ServiceStartPointQueueMode.h" +#include "ServiceStartRtMode.h" #include "ServiceStopTrajMode.h" #include "ServiceSelectMotionTool.h" #include "MotionControl.h" +#include "RealTimeMotionControl.h" #include "ConfigFile.h" #include "RosApiNameConstants.h" #include "TimeConversionUtils.h" diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj index e16072b6..8f134b57 100644 --- a/src/MotoROS2_AllControllers.vcxproj +++ b/src/MotoROS2_AllControllers.vcxproj @@ -368,6 +368,7 @@ + @@ -434,9 +435,11 @@ + + @@ -467,11 +470,13 @@ + + diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters index ba01b99c..e3b77646 100644 --- a/src/MotoROS2_AllControllers.vcxproj.filters +++ b/src/MotoROS2_AllControllers.vcxproj.filters @@ -277,6 +277,9 @@ MotoPlus Libraries\micro-ROS + + Docs\doc + @@ -369,6 +372,12 @@ Source Files\Tests + + Source Files\Services + + + Source Files\Robot Controller + @@ -485,5 +494,11 @@ Header Files\Tests + + Header Files\Services + + + Header Files\Robot Controller + \ No newline at end of file diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c new file mode 100644 index 00000000..09b5e8d0 --- /dev/null +++ b/src/RealTimeMotionControl.c @@ -0,0 +1,664 @@ +//RealTimeMotionControl.c + +// SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2025, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +// Based loosely on @adv4ncr's modifications to MotoROS1 for real-time control through ROS2. +// https://github.com/adv4ncr/motoman_ROS2/blob/cdf63a592596ff711df842680a1e4c730fd547a7/controller_driver/RealTimeMotionServer.c + +#include "MotoROS.h" + +void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData); +bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData); +bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); +bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); +void Ros_RtMotionControl_Cleanup(); +void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply); +bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools); +void Ros_RtMotionControl_PurgeBufferedPackets(); +void Ros_RtMotionControl_SendRobotStatus(); + +static int sockRtCommandListener = -1; +static int sockRtStatusSender = -1; + +static struct sockaddr_in client_addr_status_messages; + +static LONG prevRtCmdPosition[MAX_GROUPS][MAX_AXES]; +static LONG howMuchShouldIHaveMoved[MAX_GROUPS][MAX_AXES]; + +void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) +{ + MP_EXPOS_DATA moveData; + int bytes_received; + + bool bFirstRecv = true; + struct sockaddr_in client_addr; + struct sockaddr_in previous_client_addr; + int client_addr_len = sizeof(client_addr); + + RtPacket incomingCommand; + RtReply outgoingReply; + + UINT32 previousSequenceId = 0; + + struct fd_set fds; + struct timeval tv; + struct timeval* timeout; + + bool fsuLimitingDetected; + + bool packetTypeOK; + + //========================================================================================= + + bzero(prevRtCmdPosition, MAX_GROUPS * MAX_AXES * sizeof(LONG)); + bzero(howMuchShouldIHaveMoved, MAX_GROUPS * MAX_AXES * sizeof(LONG)); + + if (mode == MOTION_MODE_RT_JOINT) + Ros_RtMotionControl_InitJointSpace(&moveData); + else + { + if (!Ros_RtMotionControl_InitCartesian(&moveData)) + return; //abort rt session + } + + Ros_Debug_BroadcastMsg("Starting RT session"); + + Ros_Debug_BroadcastMsg("Flushing stale packets from socket buffer..."); + Ros_RtMotionControl_PurgeBufferedPackets(); + + //========================================================================================= + while (TRUE) + { + FD_ZERO(&fds); + FD_SET(sockRtCommandListener, &fds); + + tv.tv_usec = (g_nodeConfigSettings.timeout_for_rt_msg % 1000) * 1000; + tv.tv_sec = g_nodeConfigSettings.timeout_for_rt_msg / 1000; + + if (g_nodeConfigSettings.timeout_for_rt_msg != -1) + timeout = &tv; + else + timeout = NULL; + + if (mpSelect(sockRtCommandListener + 1, &fds, NULL, NULL, timeout) > 0) + { + bzero(&incomingCommand, sizeof(incomingCommand)); + bytes_received = mpRecvFrom(sockRtCommandListener, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); + + if (bFirstRecv) + { + previous_client_addr = client_addr; //only allow a single commander + //flag is cleared down below + + //setup destination for status messages + memset(&client_addr_status_messages, 0, sizeof(client_addr_status_messages)); + client_addr_status_messages.sin_family = AF_INET; + client_addr_status_messages.sin_addr.s_addr = client_addr.sin_addr.s_addr; + client_addr_status_messages.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_status_udp_port_number)); + + //if (mpConnect(sockRtStatusSender, (struct sockaddr*)&client_addr_status_messages, sizeof(client_addr_status_messages)) == ERROR) + //{ + // Ros_Debug_BroadcastMsg("ERROR: Failed to set destination address for RT status"); + // mpClose(sockRtStatusSender); + // sockRtStatusSender = -1; + // break; + //} + } + else + { + if (memcmp(&client_addr.sin_addr.s_addr, &previous_client_addr.sin_addr.s_addr, sizeof(UINT32)) != 0) + { + Ros_Debug_BroadcastMsg("ERROR: Received command packets from multiple sources (0x%08X and 0x%08X)", + (UINT32)previous_client_addr.sin_addr.s_addr, + (UINT32)client_addr.sin_addr.s_addr); + break; //drop the connection + } + } + + if (bytes_received == sizeof(RtPacket)) + { + //Verify version of the command packet + if (incomingCommand.version != VERSION_REAL_TIME_INTERFACE) + { + Ros_Debug_BroadcastMsg("ERROR: The command packet must be version [%d]", VERSION_REAL_TIME_INTERFACE); + break; //drop the connection + } + + //Verify the packet type + packetTypeOK = TRUE; + switch (mode) + { + case MOTION_MODE_RT_JOINT: + if (incomingCommand.packetType != PacketType_Joint_Increments) + { + packetTypeOK = false; + Ros_Debug_BroadcastMsg("ERROR: The packet type does not match the control_mode specified in start_rt_mode (Joint Increments)"); + } + break; + case MOTION_MODE_RT_CARTESIAN: + if (incomingCommand.packetType != PacketType_Cart_Increments) + { + packetTypeOK = false; + Ros_Debug_BroadcastMsg("ERROR: The packet type does not match the control_mode specified in start_rt_mode (Cartesian Increments)"); + } + break; + default: + packetTypeOK = false; + Ros_Debug_BroadcastMsg("ERROR: Unexpected motion mode is active"); + } + if (!packetTypeOK) + break; //drop the connection + + //Check for old or same sequence ID (wraparound safe) + if (((int32_t)(incomingCommand.sequenceId - previousSequenceId) <= 0) && !bFirstRecv) + { + // This packet is old or a duplicate. + Ros_Debug_BroadcastMsg("WARN: Received old command packet (prev: %u, new: %u)", previousSequenceId, incomingCommand.sequenceId); + + // Resend the previous reply and drop this packet. + mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); + continue; + } + + //Check if the sequence ID jumped too far ahead (wraparound safe) + if ((incomingCommand.sequenceId - previousSequenceId) > g_nodeConfigSettings.max_sequence_diff_for_rt_msg) + { + Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (prev: %u, new: %u)", + previousSequenceId, incomingCommand.sequenceId); + break; // Drop the connection + } + + if (mode == MOTION_MODE_RT_JOINT) + { + if (!Ros_RtMotionControl_ParseJointSpace(&incomingCommand, &moveData)) + break; //drop the connection + } + else + { + if (!Ros_RtMotionControl_ParseCartesian(&incomingCommand, &moveData)) + break; //drop the connection + } + + fsuLimitingDetected = Ros_RtMotionControl_CheckForFsuInterference(mode, incomingCommand.toolIndex); + + // Send increment to robot + int ret = mpExRcsIncrementMove(&moveData); + if (ret != OK) + { + Ros_Debug_BroadcastMsg("WARN: mpExRcsIncrementMove returned %d", ret); + break; //drop the connection + } + + bFirstRecv = false; + } + else + { + Ros_Debug_BroadcastMsg("ERROR: recvFrom returned an error"); + break; + } + + // Wait for next interpolation cycle + mpClkAnnounce(MP_INTERPOLATION_CLK); + + //send status back to the PC and notify it that I'm ready for another packet + previousSequenceId = incomingCommand.sequenceId; + Ros_RtMotionControl_PopulateReplyMessage(mode, &incomingCommand, &outgoingReply); + outgoingReply.fsuInterferenceDetected = fsuLimitingDetected; + + Ros_RtMotionControl_PurgeBufferedPackets(); //in case user sent multiple commands while this was sleeping + mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); + + //track how big the increment SHOULD have been + //we'll compare next cycle to see what actually happened + bzero(howMuchShouldIHaveMoved, MAX_GROUPS * MAX_AXES * sizeof(LONG)); + for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) + { + for (int axis = 0; axis < MAX_AXES; axis += 1) + { + howMuchShouldIHaveMoved[groupIndex][axis] = moveData.grp_pos_info[groupIndex].pos[axis]; + } + } + } + else + { + Ros_Debug_BroadcastMsg("No packets received for %d milliseconds", g_nodeConfigSettings.timeout_for_rt_msg); + break; + } + } + + Ros_Debug_BroadcastMsg("Ending Rt Session"); +} + + +void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData) +{ + int i; + MP_CTRL_GRP_SEND_DATA ctrlGroup; + MP_PULSE_POS_RSP_DATA cmdPulse; + + bzero(moveData, sizeof(MP_EXPOS_DATA)); + + for (i = 0; i < g_Ros_Controller.numGroup; i++) + { + moveData->ctrl_grp |= (0x01 << i); + moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); + moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE; + + + ctrlGroup.sCtrlGrp = g_Ros_Controller.ctrlGroups[i]->groupId;; + mpGetPulsePos(&ctrlGroup, &cmdPulse); + memcpy(prevRtCmdPosition[i], cmdPulse.lPos, sizeof(cmdPulse.lPos)); + } +} + +bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) +{ + int i; + MP_CARTPOS_EX_SEND_DATA cartSendData; + MP_CART_POS_RSP_DATA_EX cartRespData; + MP_GET_TOOL_NO_RSP_DATA getToolResp; + + bzero(moveData, sizeof(MP_EXPOS_DATA)); + + moveData->m_ctrl_grp = 0; + moveData->s_ctrl_grp = 0; + + for (i = 0; i < g_Ros_Controller.numGroup; i++) + { + CtrlGroup* group = g_Ros_Controller.ctrlGroups[i]; + + if (Ros_CtrlGroup_IsRobot(group)) //is a robot and not an external axis + cartSendData.sFrame = 1; //1 = RF + //TODO: Test on an actual track. I don't think you can get the cartesian of the track alone. + // I think that you can only get robot position, but in Base Frame. +// else if (Ros_CtrlGroup_IsBase(group)) //is a base track +// cartSendData.sFrame = 0; //0 = BF + else + { + Ros_Debug_BroadcastMsg("ERROR: Group [%d] is an external positioner. Cartesian control mode is not supported for this group.", i); + return false; + } + + moveData->ctrl_grp |= (1 << i); + moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); + moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_RF_DTYPE; + + //NOTE: This isn't the best method for this. During testing, I had tool #2 selected + // on the pendant, but I was commanding increments on tool #0. Because of this, + // the first motion on each axis would trigger the FSU detection mechanism. But + // it immediately recovers after one cycle. + mpGetToolNo(group->groupId, &getToolResp); + + cartSendData.sRobotNo = i; + + cartSendData.sToolNo = getToolResp.sToolNo; + mpGetCartPosEx(&cartSendData, &cartRespData); + memcpy(prevRtCmdPosition[i], cartRespData.lPos, sizeof(LONG) * MAX_AXES); + } + + return true; +} + +bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData) +{ + int i, groupNo; + + long pulse_increments[MAX_PULSE_AXES]; + + // For each control group, convert radians to pulses and prepare moveData + for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) + { + CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + + //joints must be in moto-order + Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(ctrlGroup, incomingCommand->delta[groupNo], pulse_increments); + + // Copy pulse increments to moveData + for (i = 0; i < ctrlGroup->numAxes; i++) + { + moveData->grp_pos_info[groupNo].pos[i] = pulse_increments[i]; + + if (abs(pulse_increments[i]) > ctrlGroup->maxInc.maxIncrement[i]) + { + Ros_Debug_BroadcastMsg("ERROR: Group [%d] Axis [%d] has been commanded to move [%d] pulse counts this increment, exceeding the maximum limit of [%d] pulse counts", groupNo, i, pulse_increments[i], ctrlGroup->maxInc.maxIncrement[i]); + return false; + } + } + + moveData->grp_pos_info[groupNo].pos_tag.data[2] = incomingCommand->toolIndex[groupNo]; + } + + return true; +} + +bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData) +{ + int groupNo; + + // For each control group, convert incoming command and prepare moveData + for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) + { + moveData->grp_pos_info[groupNo].pos_tag.data[2] = incomingCommand->toolIndex[groupNo]; + + moveData->grp_pos_info[groupNo].pos[TCP_X] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_X]); + moveData->grp_pos_info[groupNo].pos[TCP_Y] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_Y]); + moveData->grp_pos_info[groupNo].pos[TCP_Z] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_Z]); + + Quaternion q; + q.x = incomingCommand->delta[groupNo][TCP_Qx]; + q.y = incomingCommand->delta[groupNo][TCP_Qy]; + q.z = incomingCommand->delta[groupNo][TCP_Qz]; + q.w = incomingCommand->delta[groupNo][TCP_Qw]; + + LONG rx_deg = 0, ry_deg = 0, rz_deg = 0; + QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient(&q, &rx_deg, &ry_deg, &rz_deg); + + moveData->grp_pos_info[groupNo].pos[3] = rx_deg; + moveData->grp_pos_info[groupNo].pos[4] = ry_deg; + moveData->grp_pos_info[groupNo].pos[5] = rz_deg; + + moveData->grp_pos_info[groupNo].pos[6] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Re]); + + moveData->grp_pos_info[groupNo].pos[7] = 0; + + double magnitude = METERS_TO_MILLIMETERS(sqrt(pow(incomingCommand->delta[groupNo][TCP_X], 2) + //x^2 + pow(incomingCommand->delta[groupNo][TCP_Y], 2) + //y^2 + pow(incomingCommand->delta[groupNo][TCP_Z], 2))); //z^2 + + // Assuming 'elapsed_ms' is your variable for time in milliseconds. + const double max_speed_mm_per_ms = 1.5; // 1500 mm/sec is 1.5 mm/ms + + if (magnitude > (max_speed_mm_per_ms * g_Ros_Controller.interpolPeriod)) + { + Ros_Debug_BroadcastMsg("ERROR: The increment for the TCP exceeds the maximum limit of 1500 mm/sec"); + return false; + } + } + + return true; +} + +void Ros_RtMotionControl_Cleanup() +{ + //Do not close sockRtCommandListener. Allow it to persist + //indefinitely and be reused. + + //Do not delete interpolation task. This is handled in Ros_MotionControl_StopTrajMode. +} + +void Ros_RtMotionControl_OpenSocket() +{ + struct sockaddr_in server_addr; + + sockRtCommandListener = mpSocket(AF_INET, SOCK_DGRAM, 0); + if (sockRtCommandListener < 0) + { + Ros_Debug_BroadcastMsg("ERROR: Could not allocate socket for RT interface"); + motoRosAssert_withMsg(false, SUBCODE_FAIL_ALLOCATE_RT_CMD_SOCKET, "Failed to allocate RT socket"); + } + + // Bind socket to port + memset(&server_addr, 0, sizeof(server_addr)); + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = INADDR_ANY; + server_addr.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_listener_udp_port_number)); + + if (mpBind(sockRtCommandListener, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) + { + Ros_Debug_BroadcastMsg("ERROR: Failed to bind UDP socket for real-time motion control"); + mpClose(sockRtCommandListener); + sockRtCommandListener = -1; + motoRosAssert_withMsg(false, SUBCODE_FAIL_BIND_RT_SOCKET, "Failed to bind RT socket"); + } + + //========================================================================================= + sockRtStatusSender = mpSocket(AF_INET, SOCK_DGRAM, 0); + if (sockRtStatusSender < 0) + { + Ros_Debug_BroadcastMsg("ERROR: Could not allocate Status socket for RT interface"); + motoRosAssert_withMsg(false, SUBCODE_FAIL_ALLOCATE_RT_FB_SOCKET, "Failed to allocate RT socket"); + } + + //Spin up a separate normal-priorty thread to send out the robot status info + mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, + (FUNCPTR)Ros_RtMotionControl_SendRobotStatus, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); +} + +void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply) +{ + long pulsePos_moto[MAX_PULSE_AXES]; + long degrees[MP_GRP_AXES_NUM]; + BITSTRING figure; + MP_COORD coord; + MP_CTRL_GRP_SEND_DATA ctrlGroup; + MP_PULSE_POS_RSP_DATA cmdPulse; + + bzero(reply, sizeof(RtReply)); + bzero(degrees, sizeof(long) * MP_GRP_AXES_NUM); + + reply->sequenceEcho = command->sequenceId; + + for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) + { + CtrlGroup* group = g_Ros_Controller.ctrlGroups[groupIndex]; + + //================================================================================ + //FB pos + //================================================================================ + Ros_CtrlGroup_GetFBPulsePos(group, pulsePos_moto); + + //Angles (or meters for a linear track) + Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, pulsePos_moto, reply->feedbackPositionJoints[groupIndex]); + + if (Ros_CtrlGroup_IsRobot(group)) //is a robot and not an external axis + { + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + { + //if (group->axisType.type[axis] == AXIS_ROTATION) + degrees[axis] = RAD_TO_DEG_0001(reply->feedbackPositionJoints[groupIndex][axis]); + } + + //Cart + mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); + + Quaternion qFb; + QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(coord.rx, coord.ry, coord.rz, &qFb); + + reply->feedbackPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); + reply->feedbackPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); + reply->feedbackPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); + + reply->feedbackPositionCartesian[groupIndex][TCP_Qx] = qFb.x; + reply->feedbackPositionCartesian[groupIndex][TCP_Qy] = qFb.y; + reply->feedbackPositionCartesian[groupIndex][TCP_Qz] = qFb.z; + reply->feedbackPositionCartesian[groupIndex][TCP_Qw] = qFb.w; + reply->feedbackPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + } + + //================================================================================ + //CMD pos + //================================================================================ + //Should this be Ros_CtrlGroup_GetPulsePosCmd? + //Answer: No, it should not. That should only be used when converting incoming + // positional commands that contain an absolute position. + // See https://github.com/Yaskawa-Global/motoros2/discussions/455 + ctrlGroup.sCtrlGrp = g_Ros_Controller.ctrlGroups[groupIndex]->groupId; + mpGetPulsePos(&ctrlGroup, &cmdPulse); + + //rad (or meter for linear track) + Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, cmdPulse.lPos, reply->previousCommandPositionJoints[groupIndex]); + + if (Ros_CtrlGroup_IsRobot(group)) //is a robot and not an external axis + { + //deg + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + degrees[axis] = RAD_TO_DEG_0001(reply->previousCommandPositionJoints[groupIndex][axis]); + + //Cart + mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); + + Quaternion qCmd; + QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(coord.rx, coord.ry, coord.rz, &qCmd); + + reply->previousCommandPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); + reply->previousCommandPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); + reply->previousCommandPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); + + reply->previousCommandPositionCartesian[groupIndex][TCP_Qx] = qCmd.x; + reply->previousCommandPositionCartesian[groupIndex][TCP_Qy] = qCmd.y; + reply->previousCommandPositionCartesian[groupIndex][TCP_Qz] = qCmd.z; + reply->previousCommandPositionCartesian[groupIndex][TCP_Qw] = qCmd.w; + reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + } + } +} + +bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools) +{ + MP_CTRL_GRP_SEND_DATA ctrlGroup; + MP_PULSE_POS_RSP_DATA cmdPulse; + MP_CARTPOS_EX_SEND_DATA cartSendData; + MP_CART_POS_RSP_DATA_EX cartRespData; + bool returnValue = FALSE; + + for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) + { + //================================================================================ + //FSU speed limit + //================================================================================ + + LONG difference; + LONG howMuchDidIActuallyMove[MAX_AXES]; + bzero(howMuchDidIActuallyMove, sizeof(LONG) * MAX_AXES); + + if (mode == MOTION_MODE_RT_JOINT) + { + //Should this be Ros_CtrlGroup_GetPulsePosCmd? + //Answer: No, it should not. That should only be used when converting incoming + // positional commands that contain an absolute position. + // See https://github.com/Yaskawa-Global/motoros2/discussions/455 + ctrlGroup.sCtrlGrp = g_Ros_Controller.ctrlGroups[groupIndex]->groupId; + mpGetPulsePos(&ctrlGroup, &cmdPulse); + } + else if (mode == MOTION_MODE_RT_CARTESIAN) + { + cartSendData.sRobotNo = groupIndex; + cartSendData.sFrame = 1; //1 = RF + cartSendData.sToolNo = tools[groupIndex]; + mpGetCartPosEx(&cartSendData, &cartRespData); + } + + // Check if pulses (or mm's) are missing from last increment. + // Get the current controller command position and substract the previous command position + // and check if it matches the amount if increment sent last cycle + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + { + if (mode == MOTION_MODE_RT_CARTESIAN) + { + howMuchDidIActuallyMove[axis] = cartRespData.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; + prevRtCmdPosition[groupIndex][axis] = cartRespData.lPos[axis]; + } + else if (mode == MOTION_MODE_RT_JOINT) + { + howMuchDidIActuallyMove[axis] = cmdPulse.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; + prevRtCmdPosition[groupIndex][axis] = cmdPulse.lPos[axis]; + } + } + + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + { + //When working in cartesian space, we're only going to monitor the translation. + //1. There is no FSU speed limit for rotation. So it's moot. + //2. When rotating by some increment, that rotation gets 'spread out' over multiple + // axes. Even if I put all of my commanded increment into a single axis, all + // three of them are going to react. So, the cmd-value of my intended axis may + // not be the value I expect. + if (mode == MOTION_MODE_RT_CARTESIAN && axis > TCP_Z) + { + break; + } + + difference = howMuchShouldIHaveMoved[groupIndex][axis] - howMuchDidIActuallyMove[axis]; + if (abs(difference) > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) + { + //Ros_Debug_BroadcastMsg("howMuchShouldIHaveMoved[%d][%d] = %d", groupIndex, axis, howMuchShouldIHaveMoved[groupIndex][axis]); + //Ros_Debug_BroadcastMsg("howMuchDidIActuallyMove[%d] = %d", axis, howMuchDidIActuallyMove[axis]); + //Ros_Debug_BroadcastMsg("difference = %d", difference); + //Ros_Debug_BroadcastMsg("---------"); + + returnValue = TRUE; + } + } + } + return returnValue; +} + +void Ros_RtMotionControl_PurgeBufferedPackets() +{ + struct fd_set fds; + struct timeval tv; + struct sockaddr_in client_addr; + int client_addr_len = sizeof(client_addr); + RtPacket incomingCommand; + + //---------------------------- + //mpIoctl(sockRtCommandListener, FIOFLUSH, 1); + //UPDATE: mpIoctl isn't working! We'll manually purge the buffer with a draining loop. + //---------------------------- + while (TRUE) + { + FD_ZERO(&fds); + FD_SET(sockRtCommandListener, &fds); + + //no wait + tv.tv_usec = 0; + tv.tv_sec = 0; + + if (mpSelect(sockRtCommandListener + 1, &fds, NULL, NULL, &tv) > 0) + { + mpRecvFrom(sockRtCommandListener, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); + } + else + break; + } +} + +//Essentially a clone of the /robot_status topic. But decoupled from the industrial_msgs/RobotStatus type. +void Ros_RtMotionControl_SendRobotStatus() +{ + RobotState stateMsg; + + int client_addr_len = sizeof(client_addr_status_messages); + + if (g_messages_RobotStatus.msgRobotStatus == NULL) //may already be allocated in ControllerStatusIO.c + { + g_messages_RobotStatus.msgRobotStatus = industrial_msgs__msg__RobotStatus__create(); + rosidl_runtime_c__int32__Sequence__init(&g_messages_RobotStatus.msgRobotStatus->error_codes, MAX_ALARM_COUNT + 1); + } + + stateMsg.version = VERSION_OF_ROBOT_STATE_PACKET; + + while (TRUE) + { + Ros_Sleep(g_nodeConfigSettings.rt_status_sleep_period); + + //------------------------------------------------------------------------------- + stateMsg.drives_powered = g_messages_RobotStatus.msgRobotStatus->drives_powered.val; + stateMsg.e_stopped = g_messages_RobotStatus.msgRobotStatus->e_stopped.val; + stateMsg.in_motion = g_messages_RobotStatus.msgRobotStatus->in_motion.val; + stateMsg.play_mode = (g_messages_RobotStatus.msgRobotStatus->mode.val == industrial_msgs__msg__RobotMode__AUTO); + stateMsg.motion_possible = g_messages_RobotStatus.msgRobotStatus->motion_possible.val; + stateMsg.error = g_messages_RobotStatus.msgRobotStatus->in_error.val; + if (g_messages_RobotStatus.msgRobotStatus->error_codes.size > 0) + stateMsg.error_code = g_messages_RobotStatus.msgRobotStatus->error_codes.data[0]; + else + stateMsg.error_code = 0; + + //------------------------------------------------------------------------------- + mpSendTo(sockRtStatusSender, (char*)&stateMsg, sizeof(RobotState), 0, (struct sockaddr*)&client_addr_status_messages, client_addr_len); + } +} diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h new file mode 100644 index 00000000..19adaa11 --- /dev/null +++ b/src/RealTimeMotionControl.h @@ -0,0 +1,174 @@ +// RealTimeMotionControl.h + +// SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2025, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef MOTOROS2_REALTIME_MOTION_CONTROL_H +#define MOTOROS2_REALTIME_MOTION_CONTROL_H + +#define VERSION_REAL_TIME_INTERFACE 1 + +#define PACKED __attribute__ ((__packed__)) + +extern void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode); +extern void Ros_RtMotionControl_OpenSocket(); +extern void Ros_RtMotionControl_Cleanup(); + +typedef enum +{ + PacketType_Joint_Increments = 0, + PacketType_Cart_Increments +} PacketType; + +typedef enum +{ + Group_1 = 0, + Group_2, + Group_3, + Group_4, + Group_5, + Group_6, + Group_7, + Group_8, + + MAX_GROUPS +} GroupIndices; + +typedef enum +{ + Joint_S = 0, //radians + Joint_L, + Joint_U, + Joint_R, + Joint_B, + Joint_T, + Joint_E, + Joint_8, + + MAX_JOINTS +} JointIndices; + +typedef enum +{ + TCP_X = 0, //meters + TCP_Y, + TCP_Z, + + TCP_Qx, //quaternion + TCP_Qy, + TCP_Qz, + TCP_Qw, + + TCP_8, //pulse + + MAX_AXES //maxies +} CartesianIndices; + +//########################################################################## +// !All data is little-endian! +//########################################################################## + +struct RtPacket_ +{ + //The version of the command packet must match the value expected + //by MotoROS2. + int version; + + //The packet type must match the control_mode which was specified + //in when invoking the start_rt_mode service. + PacketType packetType; + + //Must increment sequentially with each new command packet. + UINT32 sequenceId; + + //The order of the joints must be in the order of [S L U R B T E 8]. + //Please note that for seven axis robots, the 'E' joint is phyically + //mounted in the middle of the arm. But it must be sent at the end + //of the joint array. See JointIndices enum. + // + //For joint-space, this will be radians of each joint. + // + //For cartesian, this will be meters and quaternion of the TCP. + //The order of the joints must be in the order of [X Y Z Qx Qy Qz Qw 8]. + //See CartesianIndices enum. + double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //Set tool that will be used by motion API (ie: passed by us to mpExRcsIncrementMove(..)) + //NOTE: this will change the 'motion tool' ONLY for those increments which + // haven't yet been added to the increment queue. See also the ROS 2 + // 'select_tool' service definition file in motoros2_interfaces. + int toolIndex[MAX_GROUPS]; //TOOL 0 - 63 + + //Reserved for future expansion + char reserved[64]; + +} PACKED; +typedef struct RtPacket_ RtPacket; + + +//########################################################################## +// !All data is little-endian! +//########################################################################## + +struct RtReply_ +{ + UINT32 sequenceEcho; + + //This is indicative of where the robot is physically located. + //Please note that this will trail behind the commanded position. + //The joint ordering will match that of the original command + //packet. See JointIndices and CartesianIndices enums. + double feedbackPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; + double feedbackPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //The command position is the target destination you are instructing + //the robot to reach. It's the calculated endpoint based on the sum + //of all position increments received from the user. + // + //This is used to track if the robot's speed is being limited + //by the Functional Safety Unit (FSU). It can also be used to + //monitor the latency between command and feedback. + double previousCommandPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; + double previousCommandPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //If the FSU speed limit is enabled, it can truncate the commanded + //delta increments. This flag is an indicator that the *previous* + //command cycle was truncated. It does NOT indicate that this most + //recent command packet was truncated. + bool fsuInterferenceDetected; +} PACKED; +typedef struct RtReply_ RtReply; + + +//########################################################################## +// !All data is little-endian! +//########################################################################## + +//Essentially a clone of the /robot_status topic. But decoupled +//from the industrial_msgs/RobotStatus type. +#define VERSION_OF_ROBOT_STATE_PACKET 1 +struct RobotState_ +{ + int version; + + BOOL drives_powered; + BOOL e_stopped; + BOOL in_motion; + BOOL play_mode; + BOOL motion_possible; + BOOL error; + int error_code; +} PACKED; +typedef struct RobotState_ RobotState; + +//When checking for interference from the FSU speed limit, there will +//likely be some small rounding errors. So, the deviation must exceed +//this amount before the system will report that the FSU has limited +//the incoming motion command. +#define MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION 20 //20 pulse, 0.020 millimeters, or 0.0020 degrees + +#undef PACKED + +#endif //MOTOROS2_REALTIME_MOTION_CONTROL_H diff --git a/src/RosApiNameConstants.h b/src/RosApiNameConstants.h index 4523d31a..3dd167e7 100644 --- a/src/RosApiNameConstants.h +++ b/src/RosApiNameConstants.h @@ -25,6 +25,7 @@ #define SERVICE_NAME_RESET_ERROR "reset_error" #define SERVICE_NAME_START_TRAJ_MODE "start_traj_mode" #define SERVICE_NAME_START_POINT_QUEUE_MODE "start_point_queue_mode" +#define SERVICE_NAME_START_RT_MODE "start_rt_mode" #define SERVICE_NAME_STOP_TRAJ_MODE "stop_traj_mode" #define SERVICE_NAME_QUEUE_TRAJ_POINT "queue_traj_point" #define SERVICE_NAME_SELECT_MOTION_TOOL "select_motion_tool" diff --git a/src/ServiceStartRtMode.c b/src/ServiceStartRtMode.c new file mode 100644 index 00000000..f297e3b9 --- /dev/null +++ b/src/ServiceStartRtMode.c @@ -0,0 +1,89 @@ +//ServiceStartRtMode.c + +// SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2025, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +#include "MotoROS.h" + +rcl_service_t g_serviceStartRtMode; + +ServiceStartRtMode_Messages g_messages_StartRtMode; + +// shorten the typename a little, locally +typedef motoros2_interfaces__srv__StartRtMode_Request StartRtMode_Request; +typedef motoros2_interfaces__srv__StartRtMode_Response StartRtMode_Response; + +void Ros_ServiceStartRtMode_Initialize() +{ + MOTOROS2_MEM_TRACE_START(svc_start_rt_mode_init); + + rcl_ret_t ret = rclc_service_init_default(&g_serviceStartRtMode, &g_microRosNodeInfo.node, + ROSIDL_GET_SRV_TYPE_SUPPORT(motoros2_interfaces, srv, StartRtMode), + SERVICE_NAME_START_RT_MODE); + motoRos_RCLAssertOK_withMsg(ret, SUBCODE_FAIL_INIT_SERVICE_START_RT_MODE, "Failed to init service (%d)", (int)ret); + + rosidl_runtime_c__String__init(&g_messages_StartRtMode.response.message); + + MOTOROS2_MEM_TRACE_REPORT(svc_start_rt_mode_init); +} + +void Ros_ServiceStartRtMode_Cleanup() +{ + MOTOROS2_MEM_TRACE_START(svc_start_rt_mode_fini); + + rcl_ret_t ret; + + Ros_Debug_BroadcastMsg("Cleanup service " SERVICE_NAME_START_RT_MODE); + ret = rcl_service_fini(&g_serviceStartRtMode, &g_microRosNodeInfo.node); + if (ret != RCL_RET_OK) + Ros_Debug_BroadcastMsg("Failed cleaning up " SERVICE_NAME_START_RT_MODE " service: %d", ret); + rosidl_runtime_c__String__fini(&g_messages_StartRtMode.response.message); + + MOTOROS2_MEM_TRACE_REPORT(svc_start_rt_mode_fini); +} + +void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg) +{ + StartRtMode_Request* request = (StartRtMode_Request*)request_msg; + StartRtMode_Response* response = (StartRtMode_Response*)response_msg; + + response->result_code.value = MOTION_READY; + rosidl_runtime_c__String__assign(&response->message, ""); + response->period = g_Ros_Controller.interpolPeriod; + response->timeout_for_rt_msg = g_nodeConfigSettings.timeout_for_rt_msg; + response->max_sequence_diff_for_rt_msg = g_nodeConfigSettings.max_sequence_diff_for_rt_msg; + + MOTION_MODE mm = MOTION_MODE_INACTIVE; + + if (request->control_mode.value == motoros2_interfaces__msg__ControlModeEnum__CARTESIAN) + mm = MOTION_MODE_RT_CARTESIAN; + else if (request->control_mode.value == motoros2_interfaces__msg__ControlModeEnum__JOINT_ANGLES) + mm = MOTION_MODE_RT_JOINT; + + if (mm != MOTION_MODE_INACTIVE) + response->result_code.value = Ros_MotionControl_StartMotionMode(mm, &response->message); + else + response->result_code.value = MOTION_NOT_READY_INVALID_SELECTION; + + if (response->result_code.value != MOTION_READY) + { + // update response + + //If it is a MOTION_NOT_READY_ERROR, then the string was already populated in the Ros_MotionControl_StartMotionMode function + if (response->result_code.value != MOTION_NOT_READY_ERROR) + { + // map to human readable string + rosidl_runtime_c__String__assign(&response->message, + Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)response->result_code.value)); + } + + Ros_Debug_BroadcastMsg("%s: %s (%d)", __func__, + response->message.data, response->result_code.value); + } + else + { + Ros_Debug_BroadcastMsg("%s: activated", __func__); + } +} diff --git a/src/ServiceStartRtMode.h b/src/ServiceStartRtMode.h new file mode 100644 index 00000000..fa58f630 --- /dev/null +++ b/src/ServiceStartRtMode.h @@ -0,0 +1,27 @@ +//ServiceStartRtMode.h + +// SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2025, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef MOTOROS2_SERVICE_START_RT_MODE_H +#define MOTOROS2_SERVICE_START_RT_MODE_H + + +extern rcl_service_t g_serviceStartRtMode; + +typedef struct +{ + motoros2_interfaces__srv__StartRtMode_Request request; + motoros2_interfaces__srv__StartRtMode_Response response; +} ServiceStartRtMode_Messages; +extern ServiceStartRtMode_Messages g_messages_StartRtMode; + +extern void Ros_ServiceStartRtMode_Initialize(); +extern void Ros_ServiceStartRtMode_Cleanup(); + +extern void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg); + + +#endif // MOTOROS2_SERVICE_START_RT_MODE_H diff --git a/src/main.c b/src/main.c index 134da58b..7c1eae50 100644 --- a/src/main.c +++ b/src/main.c @@ -105,6 +105,14 @@ void RosInitTask() Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_7, FALSE); Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_8, FALSE); + //================================== + //REUSEADDR isn't working (not officially supported anyway), so + //I need to keep the R/T socket open forever. But I also can't + //open it on a subtask which will be deleted as connections come + //and go. So, I'm opening on the one persistent task that never + //ends. + Ros_RtMotionControl_OpenSocket(); + //================================== FOREVER { @@ -134,6 +142,7 @@ void RosInitTask() Ros_ServiceResetError_Initialize(); Ros_ServiceStartTrajMode_Initialize(); Ros_ServiceStartPointQueueMode_Initialize(); + Ros_ServiceStartRtMode_Initialize(); Ros_ServiceStopTrajMode_Initialize(); Ros_ServiceSelectMotionTool_Initialize(); @@ -211,6 +220,7 @@ void RosInitTask() Ros_ServiceStopTrajMode_Cleanup(); Ros_ServiceStartTrajMode_Cleanup(); Ros_ServiceStartPointQueueMode_Cleanup(); + Ros_ServiceStartRtMode_Cleanup(); Ros_ServiceResetError_Cleanup(); Ros_ServiceReadWriteIO_Cleanup(); Ros_ServiceQueueTrajPoint_Cleanup();