Moveit Doesn't Work From C++ without a restart

[stefie10]

I am trying to send moveit commands from C++. I was working from the Python initialization code (to move to home and move to tuck) and basically did a C++ version of that with the MoveGroup class, but the arm would not move.

To try to debug it, I looked for the moveit log files on movo2, but they weren't in any obvious places. Where are they?

After failing to find the log file, I decided to try starting moveit myself so I could see what happened when I was sending the C++ service calls and if there were any errors in the log. I ran the following command:
roslaunch movo_7dof_moveit_config movo_moveit_planning_execution.launch debug:=true

Which I believe kills the moveit process that is started during bootup, then restarts it. Everything starts fine, it says "All is well! Everyone is happy! You can start planning now!", and my C++ code starts magically working. Something is clearly different between when moveit starts from boot, and when I launch it.

Here is my extracted code, which works just fine when I launch moveit myself from movo2, but does not work after a clean boot. And note that the boot itself successfully moves the arms to home, then tuck, using the Python init script.

upperBody = new MoveGroup("upper_body");
upperBody->setPlannerId("RRTConnectkConfigDefault");

for (int i = 0; i < MC->homedJoints.size(); i++) {
bool result = MC->upperBody->setJointValueTarget(MC->upperBodyJoints[i], MC->homedJoints[i]);
if (!result) {
CONSOLE_ERROR(ms, "Invalid joint target: " << MC->upperBodyJoints[i] << " value: " << MC->homedJoints[i]);
return;
}
}
MoveItErrorCode r = MC->upperBody->asyncMove();
if (r.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
CONSOLE_ERROR(ms, "Couldn't execute. Code: " << r.val);
}

[stefie10]

I found the logs. Here is the relevant excerpt from my "bad" requests. These start working as soon as I start moveit myself.

All is well! Everyone is happy! You can start planning now!

[ INFO] [1525042152.149556442]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1525042152.149764629]: Planning attempt 1 of at most 1
[ERROR] [1525042152.149865565]: Found empty JointState message
[ERROR] [1525042152.149902243]: Found empty JointState message
[ERROR] [1525042152.152991235]: Found empty JointState message
[ERROR] [1525042152.156632754]: Found empty JointState message
[ INFO] [1525042152.157221618]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1525042152.157596067]: Planner configuration 'upper_body[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1525042152.159928836]: upper_body[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1525042152.183351430]: upper_body[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1525042152.183384933]: Solution found in 0.025097 seconds
[ INFO] [1525042152.187437976]: SimpleSetup: Path simplification took 0.003984 seconds and changed from 4 to 2 states
[ INFO] [1525042162.221761843]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1525042162.221854081]: Planning attempt 1 of at most 1
[ERROR] [1525042162.221910102]: Found empty JointState message
[ERROR] [1525042162.221935953]: Found empty JointState message
[ERROR] [1525042162.223462635]: Found empty JointState message
[ERROR] [1525042162.226140633]: Found empty JointState message
[ INFO] [1525042162.226357764]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1525042162.226395341]: Planner configuration 'upper_body[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1525042162.227810619]: upper_body[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1525042162.272274526]: upper_body[RRTConnectkConfigDefault]: Created 5 states (3 start + 2 goal)
[ INFO] [1525042162.272312641]: Solution found in 0.045837 seconds
[ INFO] [1525042162.276064301]: SimpleSetup: Path simplification took 0.003707 seconds and changed from 3 to 2 states
terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injectorboost::lock_error >'
what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument
[init_robot-2] process has died [pid 3890, exit code -6, cmd /home/movo/movo_ws/src/kinova-movo/movo_robot/movo_bringup/scripts/init_robot __name:=init_robot __log:=/tmp/6bfabeaa-4bff-11e8-bb91-00215cbdfd44/init_robot-2.log].
log file: /tmp/6bfabeaa-4bff-11e8-bb91-00215cbdfd44/init_robot-2*.log
[ WARN] [1525042772.471914072]: Shutdown request received.
[ WARN] [1525042772.471957450]: Reason given for shutdown: [new node registered with same name]

[stefie10]

I figured out that this has to do with different clocks between my base station and movo. What is the correct way to sync time? If I just run ntpdate on movo1 and movo2, is that sufficient?

[martine1406]

I am not 100% sure it is relevant in your case, but you can look at this script to use chrony for time synchronization

[stefie10]

I am asking how it works on the movo platform. As of now, our movo is several hours behind "real" time. If I just plug it in and run ntpdate on movo1 and movo2, is that sufficient? Is there any time synchronization required for the arms for example?

[martine1406]

Hi @stefie10

I believe you are more knowledgeable than I am here :-O. What I can tell you is the arms do not need any time synchronization. You only have to consider the movo1 and movo2 computers for time synchronization on the movo.

Have you tested ntpdate yet? From what I see online, it seems to do what you want to do. I've never used this function command. I can ask around tomorrow in case some colleagues would have some extra insight.

Thanks and sorry I cannot answer your question completely.
Martine

[stefie10]

We will give it a shot and let you know.

[martine1406]

Hi @stefie10

Yesterday, I had a another look at the log you posted and I saw this
WARN] [1525042772.471914072]: Shutdown request received.
[ WARN] [1525042772.471957450]: Reason given for shutdown: [new node registered with same name]

Maybe your C++ initialization script and the python initialization script are conflicting? The python initialization script is called at boot up. Have you modified movo_moveit.launch to run your C++ initialization script instead?

How did you figured out it was a sync time problem?¸What made you think this was the cause of your issue?

Thank you
Martine

[martine1406]

Hi @stefie10

Can I close this issue?