From 9a94eb50b11f2595ca223467aa993862d92134bd Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Fri, 24 Feb 2017 11:09:10 +0100 Subject: [PATCH 01/14] Remove dependecy on ros_controllers metapackage. As per http://www.ros.org/reps/rep-0127.html, packages are not allowed to depend on metapackages. --- package.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 5cf0127f..11d7c958 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ 0.0.1 The new driver for the UR3/UR5/UR10 robot arms from universal robots - + Thomas Timm Andersen @@ -54,7 +54,6 @@ realtime_tools hardware_interface controller_manager - ros_controllers actionlib control_msgs geometry_msgs From b72aac3c17ecf746202c3739c7b4f718d1f6f285 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Sat, 25 Jun 2016 02:33:54 +0200 Subject: [PATCH 02/14] ur_ros_wrapper: introduce a bit of thread safety into goal handling --- src/ur_ros_wrapper.cpp | 43 +++++++++++++++++++++++++++++++++++------- 1 file changed, 36 insertions(+), 7 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ab74534b..1740d9fe 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -63,9 +63,7 @@ class RosWrapper { ros::NodeHandle nh_; actionlib::ActionServer as_; actionlib::ServerGoalHandle goal_handle_; - bool has_goal_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; + ros::Subscriber speed_sub_; ros::Subscriber urscript_sub_; ros::ServiceServer io_srv_; @@ -82,6 +80,12 @@ class RosWrapper { boost::shared_ptr hardware_interface_; boost::shared_ptr controller_manager_; + std::mutex goal_mutex_; + bool has_goal_; + std::thread traj_thread_; + control_msgs::FollowJointTrajectoryFeedback feedback_; + control_msgs::FollowJointTrajectoryResult result_; + public: RosWrapper(std::string host, int reverse_port) : as_(nh_, "follow_joint_trajectory", @@ -227,6 +231,9 @@ class RosWrapper { std::vector > velocities) { robot_.doTraj(timestamps, positions, velocities); + + std::unique_lock lock(goal_mutex_); + if (has_goal_) { result_.error_code = result_.SUCCESSFUL; goal_handle_.setSucceeded(result_); @@ -283,11 +290,22 @@ class RosWrapper { actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify + + std::unique_lock lock(goal_mutex_); if (has_goal_) { print_warning( "Received new goal while still executing previous trajectory. Canceling previous trajectory"); has_goal_ = false; - robot_.stopTraj(); + + { + lock.unlock(); + + robot_.stopTraj(); + traj_thread_.join(); + + lock.lock(); + } + result_.error_code = -100; //nothing is defined for this...? result_.error_string = "Received another trajectory"; goal_handle_.setAborted(result_, result_.error_string); @@ -373,21 +391,32 @@ class RosWrapper { goal_handle_.setAccepted(); has_goal_ = true; - std::thread(&RosWrapper::trajThread, this, timestamps, positions, - velocities).detach(); + traj_thread_ = std::thread(&RosWrapper::trajThread, this, timestamps, positions, + velocities); } void cancelCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { // set the action state to preempted + + std::unique_lock lock(goal_mutex_); + print_info("on_cancel"); if (has_goal_) { if (gh == goal_handle_) { - robot_.stopTraj(); + has_goal_ = false; + + { + lock.unlock(); + robot_.stopTraj(); + traj_thread_.join(); + lock.lock(); + } has_goal_ = false; } } + result_.error_code = -100; //nothing is defined for this...? result_.error_string = "Goal cancelled by client"; gh.setCanceled(result_); From c52eb64b78d58b55e984f5d9aec7384b3e7dda76 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Sat, 25 Jun 2016 13:29:44 +0200 Subject: [PATCH 03/14] ur_ros_wrapper: more thread safety --- src/ur_driver.cpp | 6 ++++- src/ur_ros_wrapper.cpp | 60 ++++++++++++++++++++---------------------- 2 files changed, 34 insertions(+), 32 deletions(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 22081151..3f580718 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -112,7 +112,11 @@ bool UrDriver::doTraj(std::vector inp_timestamps, executing_traj_ = false; //Signal robot to stop driverProg() UrDriver::closeServo(positions); - return true; + + if(inp_timestamps[inp_timestamps.size() - 1] < std::chrono::duration_cast>(t - t0).count()) + return true; + else + return false; } void UrDriver::servoj(std::vector positions, int keepalive) { diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 1740d9fe..d514d3a4 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -80,6 +80,9 @@ class RosWrapper { boost::shared_ptr hardware_interface_; boost::shared_ptr controller_manager_; + // Thread semantics: + // cancelCB and goalCB are both executed with locked action server mutex. + std::mutex goal_mutex_; bool has_goal_; std::thread traj_thread_; @@ -226,20 +229,22 @@ class RosWrapper { } private: - void trajThread(std::vector timestamps, + void trajThread( + std::vector timestamps, std::vector > positions, - std::vector > velocities) { - - robot_.doTraj(timestamps, positions, velocities); - - std::unique_lock lock(goal_mutex_); + std::vector > velocities) + { + bool finished = robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) { + if (finished) + { + std::unique_lock lock(goal_mutex_); result_.error_code = result_.SUCCESSFUL; goal_handle_.setSucceeded(result_); has_goal_ = false; } } + void goalCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { @@ -291,26 +296,26 @@ class RosWrapper { actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify - std::unique_lock lock(goal_mutex_); - if (has_goal_) { - print_warning( - "Received new goal while still executing previous trajectory. Canceling previous trajectory"); - has_goal_ = false; - - { - lock.unlock(); - + { + std::unique_lock lock(goal_mutex_); + if (has_goal_) { + print_warning( + "Received new goal while still executing previous trajectory. Canceling previous trajectory"); + has_goal_ = false; robot_.stopTraj(); - traj_thread_.join(); - lock.lock(); + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Received another trajectory"; + goal_handle_.setAborted(result_, result_.error_string); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); } - - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = "Received another trajectory"; - goal_handle_.setAborted(result_, result_.error_string); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); } + + if(traj_thread_.joinable()) + traj_thread_.join(); + + std::unique_lock lock(goal_mutex_); + goal_handle_ = gh; if (!validateJointNames()) { std::string outp_joint_names = ""; @@ -406,14 +411,7 @@ class RosWrapper { if (has_goal_) { if (gh == goal_handle_) { has_goal_ = false; - - { - lock.unlock(); - robot_.stopTraj(); - traj_thread_.join(); - lock.lock(); - } - has_goal_ = false; + robot_.stopTraj(); } } From 2212d9be01aca3be17b9d74b886bb186bf5b8515 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Thu, 30 Jun 2016 15:10:42 +0200 Subject: [PATCH 04/14] even more thread safety (ur_realtime_comm) --- include/ur_modern_driver/ur_realtime_communication.h | 1 + src/ur_realtime_communication.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 9d6b4454..cdec94e9 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -60,6 +60,7 @@ class UrRealtimeCommunication { public: bool connected_; RobotStateRT* robot_state_; + std::mutex mutex_; UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12); diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 90f0cfe2..b38239de 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -85,6 +85,8 @@ void UrRealtimeCommunication::halt() { } void UrRealtimeCommunication::addCommandToQueue(std::string inp) { + std::unique_lock lock(mutex_); + int bytes_written; if (inp.back() != '\n') { inp.append("\n"); From 94b1ebfe7dd25fd663df7b6eeff27f536034ac66 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Thu, 30 Jun 2016 15:15:26 +0200 Subject: [PATCH 05/14] ur_ros_wrapper: protect against simultaneous action callbacks --- src/ur_ros_wrapper.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index d514d3a4..b89ef86b 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -81,7 +81,10 @@ class RosWrapper { boost::shared_ptr controller_manager_; // Thread semantics: - // cancelCB and goalCB are both executed with locked action server mutex. + // * cancelCB and goalCB can be called simultaneously by the action server. + // We prevent that by locking the as_mutex_. + // * The has_goal_ variable is protected by the goal_mutex_, as it is + // accessed by trajThread() and the callbacks. std::mutex goal_mutex_; bool has_goal_; @@ -89,6 +92,8 @@ class RosWrapper { control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryResult result_; + std::mutex as_mutex_; + public: RosWrapper(std::string host, int reverse_port) : as_(nh_, "follow_joint_trajectory", @@ -248,6 +253,9 @@ class RosWrapper { void goalCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { + + std::unique_lock asLock(as_mutex_); + std::string buf; print_info("on_goal"); if (!robot_.sec_interface_->robot_state_->isReady()) { @@ -405,6 +413,7 @@ class RosWrapper { control_msgs::FollowJointTrajectoryAction> gh) { // set the action state to preempted + std::unique_lock asLock(as_mutex_); std::unique_lock lock(goal_mutex_); print_info("on_cancel"); From 50380df1bfce700c68ff1c862a6b2a6038e51d7a Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Wed, 22 Mar 2017 15:48:59 +0100 Subject: [PATCH 06/14] Add time parameter back to speedj for SW >= 3.3. --- src/ur_realtime_communication.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 90f0cfe2..ed56ffc6 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -98,7 +98,12 @@ void UrRealtimeCommunication::addCommandToQueue(std::string inp) { void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc) { char cmd[1024]; - if( robot_state_->getVersion() >= 3.1 ) { + if( robot_state_->getVersion() >= 3.3 ) { + sprintf(cmd, + "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.008)\n", + q0, q1, q2, q3, q4, q5, acc); + } + else if( robot_state_->getVersion() >= 3.1 ) { sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc); From e272a857e74184264f3336cf613d947aeb01a6fd Mon Sep 17 00:00:00 2001 From: Ruddick Lawrence Date: Fri, 7 Jul 2017 16:43:10 -0700 Subject: [PATCH 07/14] Copy config folder on install --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e9f3b834..f4b01d17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -133,7 +133,7 @@ endif() ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include) -include_directories(include +include_directories(include ${catkin_INCLUDE_DIRS} ) @@ -176,6 +176,7 @@ target_link_libraries(ur_driver ############# install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) +install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) ## Mark executables and/or libraries for installation install(TARGETS ur_driver ur_hardware_interface From 50510eced1e7b827ddd35d6158e5211595bf7445 Mon Sep 17 00:00:00 2001 From: Ruddick Lawrence Date: Fri, 7 Jul 2017 16:53:05 -0700 Subject: [PATCH 08/14] Correct controller names. Fixes ThomasTimm/ur_modern_driver#98 --- launch/ur3_ros_control.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch index 5b782aaf..9caf2d2a 100644 --- a/launch/ur3_ros_control.launch +++ b/launch/ur3_ros_control.launch @@ -10,7 +10,7 @@ - + @@ -38,11 +38,11 @@ + output="screen" args="joint_state_controller force_torque_sensor_controller pos_based_pos_traj_controller" /> + output="screen" args="load vel_based_pos_traj_controller" /> From 7290b1e15ad3021ec09451673bf77d84444acff8 Mon Sep 17 00:00:00 2001 From: Max Date: Fri, 4 Aug 2017 18:27:01 -0400 Subject: [PATCH 09/14] added installation and runtime execution for absolute beginners --- README.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/README.md b/README.md index 8b355ea1..48e84a54 100644 --- a/README.md +++ b/README.md @@ -49,6 +49,21 @@ The driver is designed to be a drop-in replacement of the ur\_driver package. It If you want to test it in your current setup, just use the modified launch files included in this package instead of those in ur\_bringup. Everything else should work as usual. +If you would like to run this package to connect to the hardware, you only need to run the following launch file. +``` +roslaunch ur_modern_driver urXX_bringup.launch robot_ip:=ROBOT_IP_ADDRESS +``` + +Where ROBOT_IP_ADDRESS is your UR arm's IP and XX is '5' or '10' depending on your robot. The above launch file makes calls to both roscore and the launch file to the urXX_description so that ROS's parameter server has information on your robot arm. If you do not have your ```ur_description``` installed please do so via: +``` +sudo apt install ros--ur-description +``` + +Where is the ROS distribution your machine is running on. You may want to run MoveIt to plan and execute actions on the arm. You can do so by simply entering the following commands after launching ```ur_modern_driver```: +``` +roslaunch urXX_moveit_config ur5_moveit_planning_executing.launch +roslaunch urXX_moveit_config moveit_rviz.launch config:=true +``` --- If you would like to use the ros\_control-based approach, use the launch files urXX\_ros\_control.launch, where XX is '5' or '10' depending on your robot. @@ -143,3 +158,4 @@ Please cite the following report if using this driver The report can be downloaded from http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html + From 9397f47ac158fe5976719f33101334371bc15d44 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 10 Oct 2017 14:04:55 +0200 Subject: [PATCH 10/14] Updated readme.md Updated readme.md to include information that the driver won't work reliably on a virtual machine --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 48e84a54..3dabb077 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,8 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design ## Installation +**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.** + Just clone the repository into your catkin working directory and make it with ```catkin_make```. Note that this package depends on ur_msgs, hardware_interface, and controller_manager so it cannot directly be used with ROS versions prior to hydro. From 5551cc8ce391ac4e578d7cbe669c024099ec1edd Mon Sep 17 00:00:00 2001 From: hellochenwang Date: Thu, 12 Oct 2017 22:37:58 -0700 Subject: [PATCH 11/14] Update robot_state_RT.cpp according to https://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/remote-control-via-tcpip-16496/ there's no change in the protocol from CB3.2 to CB3.4 len_good is better to default to false to ensure corrupted message doesn't get parsed. --- src/robot_state_RT.cpp | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index b170a04d..23242e9b 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -321,23 +321,23 @@ void RobotStateRT::unpack(uint8_t * buf) { len = ntohl(len); //Check the correct message length is received - bool len_good = true; - if (version_ >= 1.6 && version_ < 1.7) { //v1.6 - if (len != 756) - len_good = false; - } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 - if (len != 764) - len_good = false; - } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 - if (len != 812) - len_good = false; - } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 - if (len != 1044) - len_good = false; - } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 - if (len != 1060) - len_good = false; - } + bool len_good = false; + if (version_ >= 1.6 && version_ < 1.7) { //v1.6 + if (len == 756) + len_good = true; + } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 + if (len == 764) + len_good = true; + } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 + if (len == 812) + len_good = true; + } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 + if (len == 1044) + len_good = true; + } else if (version_ >= 3.2 && version_ < 3.5) { //v3.2 ~ v3.4 + if (len == 1060) + len_good = true; + } if (!len_good) { printf("Wrong length of message on RT interface: %i\n", len); From efed9fd376d5ed4ffb346c49915c5ba2d525a3f1 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 19 Feb 2018 07:41:35 +0100 Subject: [PATCH 12/14] Add data length for 3.5+ --- src/robot_state_RT.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 84b0221e..50ae0148 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -354,6 +354,9 @@ void RobotStateRT::unpack(uint8_t * buf) { } else if (version_ >= 3.2 && version_ < 3.5) { //v3.2 ~ v3.4 if (len == 1060) len_good = true; + } else if (version_ >= 3.5 && version_ < 3.6) { //v3.5 + if (len == 1108) + len_good = true; } if (!len_good) { From fe7424c5dbd5741c9233839892cdc598b4c59879 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 19 Feb 2018 08:54:31 +0100 Subject: [PATCH 13/14] do not print all wrong message lengths --- src/robot_state_RT.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 50ae0148..b5796674 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -360,7 +360,8 @@ void RobotStateRT::unpack(uint8_t * buf) { } if (!len_good) { - printf("Wrong length of message on RT interface: %i\n", len); + if (len != 0 && std::abs(len) <= 100000) // ignoring empty and often repeating wrong messages. + printf("Wrong length of message on RT interface: %i\n", len); val_lock_.unlock(); return; } From e353b218de3aeb2475f4c95c2b2c24326c64be33 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 19 Feb 2018 10:28:41 +0100 Subject: [PATCH 14/14] disable goal and as lock --- src/ur_ros_wrapper.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ff96f2fc..eb52ed46 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -90,13 +90,13 @@ class RosWrapper { // * The has_goal_ variable is protected by the goal_mutex_, as it is // accessed by trajThread() and the callbacks. - std::mutex goal_mutex_; + //std::mutex goal_mutex_; bool has_goal_; std::thread traj_thread_; control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryResult result_; - std::mutex as_mutex_; + //std::mutex as_mutex_; public: RosWrapper(std::string host, int reverse_port) : @@ -247,7 +247,7 @@ class RosWrapper { if (finished) { - std::unique_lock lock(goal_mutex_); + //std::unique_lock lock(goal_mutex_); result_.error_code = result_.SUCCESSFUL; goal_handle_.setSucceeded(result_); has_goal_ = false; @@ -258,7 +258,7 @@ class RosWrapper { actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { - std::unique_lock asLock(as_mutex_); + //std::unique_lock asLock(as_mutex_); std::string buf; print_info("on_goal"); @@ -309,7 +309,7 @@ class RosWrapper { *gh.getGoal(); //make a copy that we can modify { - std::unique_lock lock(goal_mutex_); + //std::unique_lock lock(goal_mutex_); if (has_goal_) { print_warning( "Received new goal while still executing previous trajectory. Canceling previous trajectory"); @@ -326,7 +326,7 @@ class RosWrapper { if(traj_thread_.joinable()) traj_thread_.join(); - std::unique_lock lock(goal_mutex_); + //std::unique_lock lock(goal_mutex_); goal_handle_ = gh; if (!validateJointNames()) { @@ -417,8 +417,8 @@ class RosWrapper { control_msgs::FollowJointTrajectoryAction> gh) { // set the action state to preempted - std::unique_lock asLock(as_mutex_); - std::unique_lock lock(goal_mutex_); + //std::unique_lock asLock(as_mutex_); + //std::unique_lock lock(goal_mutex_); print_info("on_cancel"); if (has_goal_) {