def execute_cb()

in robots/LoCoBot/locobot_control/nodes/pointrobot3factor_ros_server.py [0:0]

• 57 lines of code
• 7 McCabe index (conditional complexity)

    def execute_cb(self, goal):

        # start and end conf
        end_conf_val = np.asarray(goal.trajectory.points[0].positions)
        end_vel = np.asarray(goal.trajectory.points[0].velocities)
        goal_region_threshold = goal.goal_tolerance[0].position
        duration = goal.goal_time_tolerance.to_sec()

        print("Received goal", end_conf_val, end_vel)
        start_time = rospy.get_time()
        curstate_val, curstate_vel = self.robot.get_robot_state()
        init_distance = np.linalg.norm(curstate_val - end_conf_val)
        while np.linalg.norm(curstate_val - end_conf_val) > goal_region_threshold:

            # Timeout
            if rospy.get_time() - start_time > duration:
                rospy.logerr(
                    "The controller timedout trying to reach the goal."
                    " Consider increasing the time"
                )
                self.robot.traj_client_.cancel_goal()
                self.robot.stop()
                self._as.set_aborted()
                return

            if self._as.is_preempt_requested():
                rospy.logwarn(
                    "##############   %s: Preempted ####################"
                    % self._action_name
                )
                self._as.set_preempted()
                # Note: The trajectory is not cancelled for preempt as updated trajectory would be given
                return

            # Goal prior factors
            self.params.pose_fix_goal = noiseModel_Isotropic.Sigma(
                3,
                self.params.sigma_goal
                * np.linalg.norm(curstate_val - end_conf_val)
                / init_distance,
            )
            self.params.vel_fix_goal = noiseModel_Isotropic.Sigma(
                3,
                self.params.sigma_goal
                * np.linalg.norm(curstate_val - end_conf_val)
                / init_distance,
            )

            self.robot.sdf_lock.acquire()
            sdf = self.robot.sdf
            self.robot.sdf_lock.release()

            result, res_flag = get_plan(
                curstate_val, curstate_vel, end_conf_val, end_vel, sdf, self.params
            )

            if not res_flag:
                rospy.logerr("GPMP optimizer failed to produce an acceptable plan")
                self.robot.traj_client_.cancel_goal()
                self.robot.stop()
                self._as.set_aborted()
                return

            self.robot.executeTrajectory(result, self.params)
            rospy.sleep(0.5)

            curstate_val, curstate_vel = self.robot.get_robot_state()
            print("Current State: ", curstate_val, curstate_vel)
            print("Error", np.linalg.norm(curstate_val - end_conf_val))

        self.robot.traj_client_.wait_for_result()  # TODO: Absorb this into the treshold

        self._as.set_succeeded()