<commit_before>#include "rapid_pbd/step_executor.h"

#include <sstream>

#include "boost/shared_ptr.hpp"
#include "moveit_msgs/MoveGroupAction.h"
#include "rapid_pbd_msgs/Action.h"
#include "rapid_pbd_msgs/Step.h"
#include "tf/transform_listener.h"

#include "rapid_pbd/action_executor.h"
#include "rapid_pbd/errors.h"
#include "rapid_pbd/visualizer.h"
#include "rapid_pbd/world.h"

using boost::shared_ptr;
using rapid_pbd_msgs::Action;
using rapid_pbd_msgs::Step;

namespace rapid {
namespace pbd {
StepExecutor::StepExecutor(const rapid_pbd_msgs::Step& step,
                           ActionClients* action_clients,
                           const RobotConfig& robot_config, World* world,
                           const RuntimeVisualizer& runtime_viz,
                           const tf::TransformListener& tf_listener)
    : step_(step),
      action_clients_(action_clients),
      robot_config_(robot_config),
      world_(world),
      runtime_viz_(runtime_viz),
      motion_planning_(robot_config, world, tf_listener),
      executors_() {}

bool StepExecutor::IsValid(const rapid_pbd_msgs::Step& step) {
  for (size_t i = 0; i < step.actions.size(); ++i) {
    const Action& action = step.actions[i];
    if (!ActionExecutor::IsValid(action)) {
      ROS_ERROR("Action type %s invalid in action %ld", action.type.c_str(), i);
      return false;
    }
  }
  return true;
}

void StepExecutor::Init() {
  for (size_t i = 0; i < step_.actions.size(); ++i) {
    Action action = step_.actions[i];
    shared_ptr<ActionExecutor> ae(
        new ActionExecutor(action, action_clients_, &motion_planning_, world_,
                           robot_config_, runtime_viz_));
    executors_.push_back(ae);
  }
}

std::string StepExecutor::Start() {
  motion_planning_.ClearGoals();
  std::string error("");
  for (size_t i = 0; i < step_.actions.size(); ++i) {
    error = executors_[i]->Start();
    if (error != "") {
      return error;
    }
  }
  if (motion_planning_.num_goals() > 0) {
    moveit_msgs::MoveGroupGoal goal;
    motion_planning_.BuildGoal(&goal);
    action_clients_->moveit_client.sendGoal(goal);
  }
  return "";
}

bool StepExecutor::IsDone(std::string* error) const {
  for (size_t i = 0; i < executors_.size(); ++i) {
    const shared_ptr<ActionExecutor>& executor = executors_[i];
    if (!executor->IsDone(error)) {
      return false;
    }
    if (*error != "") {
      return true;
    }
  }
  if (motion_planning_.num_goals() > 0) {
    if (!action_clients_->moveit_client.getState().isDone()) {
      return false;
    }
    moveit_msgs::MoveGroupResultConstPtr result =
        action_clients_->moveit_client.getResult();
    if (result->error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
      std::stringstream ss;
      ss << errors::kUnreachablePose
         << " MoveIt error code: " << result->error_code.val;
      *error = ss.str();
    }
  }
  return true;
}

void StepExecutor::Cancel() {
  for (size_t i = 0; i < executors_.size(); ++i) {
    const shared_ptr<ActionExecutor>& executor = executors_[i];
    executor->Cancel();
  }<commit_msg>Added error message in case result pointer is null.<commit_after>#include "rapid_pbd/step_executor.h"

#include <sstream>

#include "boost/shared_ptr.hpp"
#include "moveit_msgs/MoveGroupAction.h"
#include "rapid_pbd_msgs/Action.h"
#include "rapid_pbd_msgs/Step.h"
#include "tf/transform_listener.h"

#include "rapid_pbd/action_executor.h"
#include "rapid_pbd/errors.h"
#include "rapid_pbd/visualizer.h"
#include "rapid_pbd/world.h"

using boost::shared_ptr;
using rapid_pbd_msgs::Action;
using rapid_pbd_msgs::Step;

namespace rapid {
namespace pbd {
StepExecutor::StepExecutor(const rapid_pbd_msgs::Step& step,
                           ActionClients* action_clients,
                           const RobotConfig& robot_config, World* world,
                           const RuntimeVisualizer& runtime_viz,
                           const tf::TransformListener& tf_listener)
    : step_(step),
      action_clients_(action_clients),
      robot_config_(robot_config),
      world_(world),
      runtime_viz_(runtime_viz),
      motion_planning_(robot_config, world, tf_listener),
      executors_() {}

bool StepExecutor::IsValid(const rapid_pbd_msgs::Step& step) {
  for (size_t i = 0; i < step.actions.size(); ++i) {
    const Action& action = step.actions[i];
    if (!ActionExecutor::IsValid(action)) {
      ROS_ERROR("Action type %s invalid in action %ld", action.type.c_str(), i);
      return false;
    }
  }
  return true;
}

void StepExecutor::Init() {
  for (size_t i = 0; i < step_.actions.size(); ++i) {
    Action action = step_.actions[i];
    shared_ptr<ActionExecutor> ae(
        new ActionExecutor(action, action_clients_, &motion_planning_, world_,
                           robot_config_, runtime_viz_));
    executors_.push_back(ae);
  }
}

std::string StepExecutor::Start() {
  motion_planning_.ClearGoals();
  std::string error("");
  for (size_t i = 0; i < step_.actions.size(); ++i) {
    error = executors_[i]->Start();
    if (error != "") {
      return error;
    }
  }
  if (motion_planning_.num_goals() > 0) {
    moveit_msgs::MoveGroupGoal goal;
    motion_planning_.BuildGoal(&goal);
    action_clients_->moveit_client.sendGoal(goal);
  }
  return "";
}

bool StepExecutor::IsDone(std::string* error) const {
  for (size_t i = 0; i < executors_.size(); ++i) {
    const shared_ptr<ActionExecutor>& executor = executors_[i];
    if (!executor->IsDone(error)) {
      return false;
    }
    if (*error != "") {
      return true;
    }
  }
  if (motion_planning_.num_goals() > 0) {
    if (!action_clients_->moveit_client.getState().isDone()) {
      return false;
    }
    moveit_msgs::MoveGroupResultConstPtr result =
        action_clients_->moveit_client.getResult();
    if (!result) {
      *error = "MoveIt returned null result.";
    }
    if (result->error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
      std::stringstream ss;
      ss << errors::kUnreachablePose
         << " MoveIt error code: " << result->error_code.val;
      *error = ss.str();
    }
  }
  return true;
}

void StepExecutor::Cancel() {
  for (size_t i = 0; i < executors_.size(); ++i) {
    const shared_ptr<ActionExecutor>& executor = executors_[i];
    executor->Cancel();
  }
{'fraction_non_alphanumeric': 0.13783269961977188, 'fraction_numerical': 0.002217997465145754, 'mean_word_length': 3.3092150170648464, 'pattern_counts': {'":': 0, '<': 33, '<?xml version=': 0, '>': 31, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3722403', 'n_tokens_mistral': 2165, 'n_tokens_neox': 2143, 'n_words': 409}