Goal request rejected

[yguel]

🕊️ 🕊️ 🕊️ 🕊️ 🕊️ 🕊️ Thanks for taking the time to fill out this issue report! 🕊️ 🕊️ 🕊️ 🕊️ 🕊️ 🕊️

Summarize
With a TM12S and humble version, we got a new strange behaviour. When planning with moveit, we always have the error «Goal was rejected by server» even when we control only the angle on one joint.

Contact details
yguel@unistra.fr and can you put in cc emvolo@irepa-laser.com

TM App Version
What version of our software are you running?

• FW 2.16.3500 and the version committed on GitHub tmr_ros2 humble 1.3.1 and 1.2.7

To Reproduce
Steps to reproduce the bug:

1. On the TMFlow robot pad of our TM12S: play the script with the node listen as described in the documentation
2. In one terminal do:

export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 run tm_driver tm_driver robot_ip:=10.42.0.172

3. In a second terminal do:

export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 launch tm12s_moveit_config tm12s_run_move_group.launch.py robot_ip:=10.42.0.172

4. Choose a planner in the moveit interface of rviz2 (e.g. RRT, etc. we tested with several)
5. Move the robot with the rviz2 handle
6. Plan & execute or Plan then Execute

Plan always works, execute always fail.

The errors in the console are:

[tm_driver-1] [INFO] [1742901121.887847875] [rclcpp]: Received new action goal 1613bff9aeb728da5daf2e53117f5d19
[move_group-5] [ERROR] [1742901121.887989928] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal was rejected by server
[move_group-5] [INFO] [1742901121.887990083] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: tmr_arm_controller started execution
[move_group-5] [WARN] [1742901121.888039097] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request rejected
[move_group-5] [ERROR] [1742901121.888019217] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller tmr_arm_controller
[move_group-5] [INFO] [1742901121.888056532] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-5] [ERROR] [1742901121.888069975] [moveit_simple_controller_manager.rclcpp_action]: unknown goal response, ignoring...
[move_group-5] [INFO] [1742901121.897497484] [moveit_move_group_default_capabilities.move_action_capability]: Solution found but controller failed during execution
[rviz2-2] [INFO] [1742901121.897952471] [move_group_interface]: Plan and Execute request aborted

Reproducibility
Describe how frequently the bug occurs.
The bug occured all the times (25x) except once.

Expected behavior/code
It should work and the robot should move at it had before.

Relevant logs and screenshots
See above

Environment

• Robot Device: real TM12S robot
• Your OS and Version: Ubuntu 22.04
• ROS insatll: ROS Humble
• MoveIt install: tested with 2 versions
  • commit c705c2cd302613d9a28f96335754d27f83966d5b (HEAD -> humble, origin/humble)
    Author: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com>
    Date: Mon Jan 13 08:32:30 2025 +0000
  • 2.5.8-1jammy.20250210

Additional context

[TechmanRobotIncOwner]

Hi, yguel sir,

Running two tm ros drivers at the same IP address is not allowed. Since the tm driver node has been written into the moveit launch file, there is no need to execute ros2 run tm_driver tm_driver robot_ip:=10.42.0.172 . Please press CTRL + C in all terminal windows to shut everything down.

Then, in one terminal do:
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
source ./install/setup.bash
ros2 launch tm12s_moveit_config tm12s_run_move_group.launch.py robot_ip:=10.42.0.17

Hope this solves your problem.

[yguel]

Dear TechmanRobotIncOwner,
We did not mention it but we tried also without the tm_driver node but the problem is the same.

It looks more related to a moveit config than a communication problem : it said goal rejected not anything about communication.

Furthermore, it used to work with or without the tm_driver before January.
I already tested to downgrade tm2_ros2 but it didn't work.
I will try to downgrade moveit. Do you have the last version of moveit that you tested successfully with humble and TM12S ?

• all the best

[TechmanRobotIncOwner]

Hi, yguel sir,

We check using tm2_ros2 humble 1.3.1 (latest version) and MoveIt binary installation: 2.5.8.
The test still succeeded just now and failed to reproduce the problem you described.

[tm_driver-1] [INFO] [1742981362.457180169] [rclcpp]: [ACTION] Received new action goal cb8ec1a7aaabb3b0ecfdec1a383a354
[tm_driver-1] [INFO] [1742981362.457196165] [rclcpp]: [ACTION] -----GOAL ACCEPT-----
[tm_driver-1] [INFO] [1742981362.457242884] [rclcpp]: [ACTION] goals_queue_ size: 1
[tm_driver-1] [INFO] [1742981362.457305961] [rclcpp]: [ACTION] -----goal execution thread begin-----
[tm_driver-1] [INFO] [1742981362.457357208] [rclcpp]: [ACTION] traj. total time:= 4
[move_group-5] [INFO] [1742981362.457377300] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: tmr_arm_controller started execution
[move_group-5] [INFO] [1742981362.457392122] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[tm_driver-1] [INFO] [1742981362.457498219] [rclcpp]: pvts points size: 44
[tm_driver-1] [INFO] [1742981362.457504398] [rclcpp]: TM_DRV: send script (pvt traj.):
[tm_driver-1] 
[tm_driver-1] [INFO] [1742981362.457521036] [rclcpp]: PVTEnter(0)
[tm_driver-1] PVTPoint(-0.00990,-0.01316,0.24548,0.05302,0.29218,-0.01071,-0.22614,-0.35265,4.93979,1.14247,5.72958,-0.22560,0.10000)
[tm_driver-1] PVTPoint(-0.04382,-0.06606,0.98645,0.22439,1.15161,-0.04455,-0.45228,-0.70529,9.87958,2.28494,11.45916,-0.45120,0.10000)
[tm_driver-1] [INFO] [1742981362.457603862] [rclcpp]: $TMSCT,5529,PvtTraj,PVTEnter(0)
[tm_driver-1] PVTPoint(-0.00990,-0.01316,0.24548,0.05302,0.29218,-0.01071,-0.22614,-0.35265,4.93979,1.14247,5.72958,-0.22560,0.10000)
[tm_driver-1] PVTPoint(-0.04382,-0.06606,0.98645,0.22439,1.15161,-0.04455,-0.45228,-0.70529,9.87958,2.28494,11.4591
[tm_driver-1] [INFO] [1742981362.457658605] [rclcpp]: $TMSCT,17,Tag,QueueTag(2,0),*25
[tm_driver-1] 
[tm_driver-1] [INFO] [1742981362.457667391] [rclcpp]: [trajectory thread] send QueueTag(2)
[tm_driver-1] [INFO] [1742981380.066204420] [rclcpp]: TM_ROS: (TM_SCT): MSG: (PvtTraj): OK
[tm_driver-1] [INFO] [1742981380.066319277] [rclcpp]: TM_ROS: (TM_SCT): MSG: (Tag): OK
[tm_driver-1] [INFO] [1742981380.081774485] [rclcpp]: [check tag] check tag: 2 status: 1
[tm_driver-1] [INFO] [1742981380.081804210] [rclcpp]: TM_ROS: (TM_STA): res: (01): 02,true
[tm_driver-1] [INFO] [1742981380.082188583] [rclcpp]: [trajectory thread] get QueueTag(2), traj DONE !
[tm_driver-1] [INFO] [1742981380.082207771] [rclcpp]: TM_DRV: traj. exec. time:= 17.625
[tm_driver-1] [INFO] [1742981380.082222400] [rclcpp]: Goal reached, success!
[tm_driver-1] [INFO] [1742981380.082328429] [rclcpp]: [ACTION] -----goal execution thread end-----

[yguel]

Many thanks for your tests.
I will continue investigating where the problem comes from. I will post here how it goes.

[yguel]

I identified where the goal was rejected. It is in the function:
src/tm2_ros2/tm_driver/src/tm_ros2_movit_sct.cpp
the condition sct_.is_connected() is always false.

[yguel]

I may add that it looks that the the sct listen node is always trying to reconnect (however, we have correct and real-time robot pose readings).
The logs of the tm_driver are as follow (note that I added some debugging information that you will not have):

[INFO] [1743080530.003693670] [rclcpp]: Ethernet slave communication: TmSvrCommunication
[INFO] [1743080530.003750998] [rclcpp]: Listen node communication: TmSctCommunication
[INFO] [1743080530.012762143] [rclcpp]: Ethernet slave communication: start
[INFO] [1743080530.012787749] [rclcpp]: TM_COM: ip:=172.16.224.152
[INFO] [1743080530.013251258] [rclcpp]: TM_COM (Ethernet slave communication): TM robot is connected. sockfd:=10
[INFO] [1743080530.013353084] [rclcpp]: TM_ROS: get data thread begin
[INFO] [1743080530.013734927] [rclcpp]: Listen node communication: start
[INFO] [1743080530.014394997] [rclcpp]: TM_COM (listen node communication): TM robot is connected. sockfd:=11
[INFO] [1743080530.016437068] [rclcpp]: $TMSCT,17,Tag,QueueTag(1,0),*26

[INFO] [1743080530.064538394] [rclcpp]: TM_ROS: sct_response thread begin
[INFO] [1743080530.066694611] [rclcpp]: $TMSTA,3,00,,*6c

[INFO] [1743080530.103043021] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[INFO] [1743080530.103232552] [rclcpp]: TM Flow DataTable Checked Item: 
[INFO] [1743080530.103268605] [rclcpp]: Total 47 item,47 checked, 0 skipped
[INFO] [1743080530.103276169] [rclcpp]: data table is correct!
[INFO] [1743080531.066856765] [rclcpp]: TM_ROS: Not on listen node!
[INFO] [1743080533.104178865] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[INFO] [1743080533.104222599] [rclcpp]: $TMSTA,3,00,,*6c

[INFO] [1743080533.165746326] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[INFO] [1743080534.104396324] [rclcpp]: TM_ROS: Not on listen node!
[INFO] [1743080536.166420361] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[INFO] [1743080536.166515188] [rclcpp]: $TMSTA,3,00,,*6c

[INFO] [1743080536.228528587] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[INFO] [1743080537.166612347] [rclcpp]: TM_ROS: Not on listen node!
[INFO] [1743080539.229439675] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[INFO] [1743080539.229506643] [rclcpp]: $TMSTA,3,00,,*6c

[INFO] [1743080539.290850475] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[INFO] [1743080540.229611354] [rclcpp]: TM_ROS: Not on listen node!
[INFO] [1743080542.291027298] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[INFO] [1743080542.291118527] [rclcpp]: $TMSTA,3,00,,*6c

...

[INFO] [1743080714.796337420] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[INFO] [1743080715.734863144] [rclcpp]: TM_ROS: Not on listen node!
[INFO] [1743080717.797337488] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[INFO] [1743080717.797388171] [rclcpp]: $TMSTA,3,00,,*6c

[INFO] [1743080717.858278584] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[INFO] [1743080718.797538273] [rclcpp]: TM_ROS: Not on listen node!
[INFO] [1743080719.756752592] [rclcpp]: [ACTION] Received new action goal 7afb711bfd7a7b507c35d26976517f
[INFO] [1743080719.756823900] [rclcpp]: [ACTION] -----GOAL REJECT-----
[INFO] [1743080719.756830919] [rclcpp]: [ACTION] TM_ROS: TMFlow (TmSctCommunication) is not connected
[INFO] [1743080720.859537380] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[INFO] [1743080720.859638464] [rclcpp]: $TMSTA,3,00,,*6c

[yguel]

In src/tm2_ros2/tm_driver/src/tm_listen_node_connect.cpp, in the function send_data, what we got is that :
rc is always in the TmCommRC::ERR state

auto rc = sct.recv_spin_once(1000, &n);
    if (rc == TmCommRC::ERR ||
        rc == TmCommRC::NOTREADY ||
        rc == TmCommRC::NOTCONNECT)
    {
        print_info("TM_ROS: (Listen node) recv_spin_once error");
        print_info("rc == ERR : %d", rc == TmCommRC::ERR);
        print_info("rc == NOTREADY : %d", rc == TmCommRC::NOTREADY);
        print_info("rc == NOTCONNECT : %d", rc == TmCommRC::NOTCONNECT);
        return false;
    }

my logs looks like:

[tm_driver-1] [INFO] [1743082687.056103917] [rclcpp]: TM_ROS: (Listen node) recv_spin_once error
[tm_driver-1] [INFO] [1743082687.056131880] [rclcpp]: rc == ERR : 1
[tm_driver-1] [INFO] [1743082687.056135480] [rclcpp]: rc == NOTREADY : 0
[tm_driver-1] [INFO] [1743082687.056138689] [rclcpp]: rc == NOTCONNECT : 0
[tm_driver-1] [INFO] [1743082687.056154857] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[tm_driver-1] [INFO] [1743082687.995394638] [rclcpp]: TM_ROS: Not on listen node!
[tm_driver-1] [INFO] [1743082690.056863269] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[tm_driver-1] [INFO] [1743082690.056948232] [rclcpp]: $TMSTA,3,00,,*6c
[tm_driver-1] 
[tm_driver-1] [INFO] [1743082690.117954449] [rclcpp]: TM_ROS: (Listen node) recv_spin_once error
[tm_driver-1] [INFO] [1743082690.117982463] [rclcpp]: rc == ERR : 1
[tm_driver-1] [INFO] [1743082690.117985946] [rclcpp]: rc == NOTREADY : 0
[tm_driver-1] [INFO] [1743082690.117988821] [rclcpp]: rc == NOTCONNECT : 0
[tm_driver-1] [INFO] [1743082690.118006469] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[tm_driver-1] [INFO] [1743082691.057044277] [rclcpp]: TM_ROS: Not on listen node!
[tm_driver-1] [INFO] [1743082693.119093214] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[tm_driver-1] [INFO] [1743082693.119241331] [rclcpp]: $TMSTA,3,00,,*6c
[tm_driver-1] 
[tm_driver-1] [INFO] [1743082693.179810405] [rclcpp]: TM_ROS: (Listen node) recv_spin_once error
[tm_driver-1] [INFO] [1743082693.179836760] [rclcpp]: rc == ERR : 1
[tm_driver-1] [INFO] [1743082693.179841486] [rclcpp]: rc == NOTREADY : 0
[tm_driver-1] [INFO] [1743082693.179844470] [rclcpp]: rc == NOTCONNECT : 0
[tm_driver-1] [INFO] [1743082693.179878025] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[tm_driver-1] [INFO] [1743082694.119359216] [rclcpp]: TM_ROS: Not on listen node!
[tm_driver-1] [INFO] [1743082696.180153130] [rclcpp]: TM_COM (Listen node): TM robot is connected. sockfd:=11
[tm_driver-1] [INFO] [1743082696.180268566] [rclcpp]: $TMSTA,3,00,,*6c
[tm_driver-1] 
[tm_driver-1] [INFO] [1743082696.241628676] [rclcpp]: TM_ROS: (Listen node) recv_spin_once error
[tm_driver-1] [INFO] [1743082696.243020951] [rclcpp]: rc == ERR : 1
[tm_driver-1] [INFO] [1743082696.243033971] [rclcpp]: rc == NOTREADY : 0
[tm_driver-1] [INFO] [1743082696.243037222] [rclcpp]: rc == NOTCONNECT : 0
[tm_driver-1] [INFO] [1743082696.243054613] [rclcpp]: TM_ROS: (Listen node): Reconnecting...
[tm_driver-1] [INFO] [1743082697.180383590] [rclcpp]: TM_ROS: Not on listen node!

[yguel]

I have pinned the error to the call to recv in function in spin_once method

TmCommRC TmCommRecv::spin_once(int timeval_ms, int *n)

of src/tm2_ros2/tm_driver/src/tm_communication.cpp line 217

The error code is 104, it corresponds to ECONNRESET on Linux systems. This error indicates that the connection was forcibly closed by the peer (i.e., the server or remote host).

So it looks that something is closing the connection in the robot command cabinet.

How can I check what is closing the connection and/or prevent it ?

[yguel]

Dear TechmanRobotIncOwner,
did you test your code with a TM12S ?

The behaviour is very erratic, when the error 104 does not appear, it works, but error 104 appears randomly 2/3 of the time now. I am quite sure that the problem is from the way sockets are handled from the command cabinet.
Do you have the socket setup on the command cabinet, maybe there is a need to match socket configurations.

On our side, it is a blocking problem and critical for our developments, we have a deadline on Monday, March the 31th.

[TechmanRobotIncOwner]

Hi, yguel sir,

From the logs you provided, it seems that the connection with the TMflow Listen node failed this time.

Please try to reconfigure the network settings between your pc environment and TMflow listener node settings.
For example, set the subnet mask on the control box to 255.255.255.0 and try to change it to "another different IP address", then turn off ⇒ turn on the robot's "Ethernet Slave Data Table" settings to reconfigure the hardware settings.

[yguel]

Dear TechmanRobotIncOwner,
Thank you for your prompt answer. Tests will be conducted on Monday with the changes you propose.
I am sure that this was the problem we had from the beginning. The robot was indeed changed from one network to another regularly.
Is there a way to have better logs from the TMflow listener stating that there is a network problem ?
Because we had nothing in the logs of the TMflow listener and nothing in the GUI of the tablet to provide us with an hint that there was a network problem.
Do you have some specifications or some code of what the TMflow listener is doing, that will help us design an industrial solution encompassing most if not all network related problems ?

I keep the changes we made to tm2_ros2 to debug in the following fork: https://github.com/yguel/tm2_ros2 (branch humble).
If you have guidelines about the process, I am willing to commit some time to propose you a proper pull request.

• all the best,

[TechmanRobotDev]

Hi @yguel ,

Nice to meet you, I am an application engineer of Techman. I think the key to this issue is to figure out what is going on with the Listen node. Here are some suggestions:

• Does the project-run stay at Listen node? For example, set the connection timeout and data timeout as 0.
• Does the project-run be paused unexpectedly? For example, (1) paused by Safety Input, (2) Safety Settings --> MODE --> "Suspend the Enabling Switch" is disabled, which means the project will be paused if the enabling switch is not hold.
• If the measures above do not help, could you reproduce the issue on your site, export the Log and your project (with Listen node) from the robot, and send it to us (it would be appreciated to use cloud link instead of email attachment)

PS: just in case you might not know how to export stuffs from the robot IPC, please check this tutorial:
https://owncloud.tm-robot.com/owncloud/index.php/s/oKK2UdzO1E4E9hT
password:!QAZ2wsx

[yguel]

Dear TechmanRobotIncOwner,

We tried your suggestions, but it did not change anything.

1. set the subnet mask on the control box to 255.255.255.0: it was already the case
2. turn off ⇒ turn on the robot's "Ethernet Slave Data Table" settings to reconfigure the hardware settings -> it did not changed anything (we had to deactivate/reactivate ROS parameter prior to do that because the checkbox for Ethernet Slave Data Table was otherwise inactivated)
3. change the ip address -> it did not changed anything, we tried with 2 different IP addresses and DHCP server (anytime we saw the correct robot configuration in RVIZ2, but we were still unable to send a goal 2/3 of the times).

I should mention that we also tried to shutdown/restart the robot several times.

Could we have a way to see detailed logs of the process closing the connection on the Omron command cabinet ?

• all the best,

[TechmanRobotIncOwner]

Hi @yguel ,

Please refer to TechmanRobotDev's reply this morning.
There are some suggestions, we discussed this with Techman's application engineer.

1. Does the project-run stay at the Listen node? For example, set the connection timeout and data timeout as 0.
2. Does the project-run be paused unexpectedly? For example, (1) paused by Safety Input, (2) Safety Settings --> MODE --> "Suspend the Enabling Switch" is disabled, which means the project will be paused if the enabling switch is not held.

We're sorry that we couldn't resolve your issue immediately.
3.If the problem persists, please export stuffs from the robot IPC, and we will check why the listener node is not ready.

Thanks