<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, PickNik LLC
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of PickNik LLC nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Henning Kayser */

#include <stdexcept>
#include <moveit/moveit_cpp/moveit_cpp.h>

namespace moveit_cpp
{
constexpr char LOGNAME[] = "moveit_cpp";
constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin";

MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
  : MoveItCpp(Options(nh), nh, tf_buffer)
{
}

MoveItCpp::MoveItCpp(const Options& options, const ros::NodeHandle& nh,
                     const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
  : tf_buffer_(tf_buffer), node_handle_(nh)
{
  if (!tf_buffer_)
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>();
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

  // Configure planning scene monitor
  if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options))
  {
    const std::string error = "Unable to configure planning scene monitor";
    ROS_FATAL_STREAM_NAMED(LOGNAME, error);
    throw std::runtime_error(error);
  }

  robot_model_ = planning_scene_monitor_->getRobotModel();
  if (!robot_model_)
  {
    const std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
                              "parameter server.";
    ROS_FATAL_STREAM_NAMED(LOGNAME, error);
    throw std::runtime_error(error);
  }

  bool load_planning_pipelines = true;
  if (load_planning_pipelines && !loadPlanningPipelines(options.planning_pipeline_options))
  {
    const std::string error = "Failed to load planning pipelines from parameter server";
    ROS_FATAL_STREAM_NAMED(LOGNAME, error);
    throw std::runtime_error(error);
  }

  // TODO(henningkayser): configure trajectory execution manager
  trajectory_execution_manager_ = std::make_shared<trajectory_execution_manager::TrajectoryExecutionManager>(
      robot_model_, planning_scene_monitor_->getStateMonitor());

  ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp running");
}

MoveItCpp::~MoveItCpp()
{
  ROS_INFO_NAMED(LOGNAME, "Deleting MoveItCpp");
  clearContents();
}

bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options)
{
  planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(options.robot_description,
                                                                                tf_buffer_, options.name);
  // Allows us to sycronize to Rviz and also publish collision objects to ourselves
  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor");
  if (planning_scene_monitor_->getPlanningScene())
  {
    // Start state and scene monitors
    ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", options.joint_state_topic.c_str());
    // Subscribe to JointState sensor messages
    planning_scene_monitor_->startStateMonitor(options.joint_state_topic, options.attached_collision_object_topic);
    // Publish planning scene updates to remote monitors like RViz
    planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                          options.monitored_planning_scene_topic);
    // Monitor and apply planning scene updates from remote publishers like the PlanningSceneInterface
    planning_scene_monitor_->startSceneMonitor(options.publish_planning_scene_topic);
    // Monitor requests for changes in the collision environment
    planning_scene_monitor_->startWorldGeometryMonitor();
  }
  else
  {
    ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured");
    return false;
  }

  // Wait for complete state to be recieved
  if (options.wait_for_initial_state_timeout > 0.0)
  {
    return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(),
                                                                           options.wait_for_initial_state_timeout);
  }

  return true;
}

bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
{
  ros::NodeHandle node_handle(options.parent_namespace.empty() ? "~" : options.parent_namespace);
  for (const auto& planning_pipeline_name : options.pipeline_names)
  {
    if (planning_pipelines_.count(planning_pipeline_name) > 0)
    {
      ROS_WARN_NAMED(LOGNAME, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str());
      continue;
    }
    ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str());
    ros::NodeHandle child_nh(node_handle, planning_pipeline_name);
    planning_pipeline::PlanningPipelinePtr pipeline;
    pipeline = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, child_nh, PLANNING_PLUGIN_PARAM);

    if (!pipeline->getPlannerManager())
    {
      ROS_ERROR_NAMED(LOGNAME, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str());
      continue;
    }

    planning_pipelines_[planning_pipeline_name] = pipeline;
  }

  if (planning_pipelines_.empty())
  {
    ROS_ERROR_NAMED(LOGNAME, "Failed to load any planning pipelines.");
    return false;
  }

  // Retrieve group/pipeline mapping for faster lookup
  std::vector<std::string> group_names = robot_model_->getJointModelGroupNames();
  for (const auto& pipeline_entry : planning_pipelines_)
  {
    for (const auto& group_name : group_names)
    {
      groups_pipelines_map_[group_name] = {};
      const auto& pipeline = pipeline_entry.second;
      for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations())
      {
        if (planner_configuration.second.group == group_name)
        {
          groups_pipelines_map_[group_name].insert(pipeline_entry.first);
        }
      }
    }
  }

  return true;
}

moveit::core::RobotModelConstPtr MoveItCpp::getRobotModel() const
{
  return robot_model_;
}

const ros::NodeHandle& MoveItCpp::getNodeHandle() const
{
  return node_handle_;
}

bool MoveItCpp::getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds)
{
  if (wait_seconds > 0.0 &&
      !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds))
  {
    ROS_ERROR_NAMED(LOGNAME, "Did not receive robot state");
    return false;
  }
  {  // Lock planning scene
    planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor_);
    current_state = std::make_shared<moveit::core::RobotState>(scene->getCurrentState());
  }  // Unlock planning scene
  return true;
}

moveit::core::RobotStatePtr MoveItCpp::getCurrentState(double wait)
{
  moveit::core::RobotStatePtr current_state;
  getCurrentState(current_state, wait);
  return current_state;
}

const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::getPlanningPipelines() const
{
  return planning_pipelines_;
}

std::set<std::string> MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const
{
  std::set<std::string> result_names;
  if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0)
  {
    ROS_ERROR_NAMED(LOGNAME,
                    "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
                    group_name.c_str());
    return result_names;  // empty
  }
  for (const auto& pipeline_entry : planning_pipelines_)
  {
    const std::string& pipeline_name = pipeline_entry.first;
    // If group_name is defined and valid, skip pipelines that don't belong to the planning group
    if (!group_name.empty())
    {
      const auto& group_pipelines = groups_pipelines_map_.at(group_name);
      if (group_pipelines.find(pipeline_name) == group_pipelines.end())
        continue;
    }
    result_names.insert(pipeline_name);
  }
  // No valid planning pipelines
  if (result_names.empty())
    ROS_ERROR_NAMED(LOGNAME,
                    "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
                    group_name.c_str());
  return result_names;
}

const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const
{
  return planning_scene_monitor_;
}

planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonitorNonConst()
{
  return planning_scene_monitor_;
}

const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveItCpp::getTrajectoryExecutionManager() const
{
  return trajectory_execution_manager_;
}

trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajectoryExecutionManagerNonConst()
{
  return trajectory_execution_manager_;
}

bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
                        bool blocking)
{
  if (!robot_trajectory)
  {
    ROS_ERROR_NAMED(LOGNAME, "Robot trajectory is undefined");
    return false;
  }

  // Check if there are controllers that can handle the execution
  if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name))
  {
    ROS_ERROR_NAMED(LOGNAME, "Execution failed! No active controllers configured for group '%s'", group_name.c_str());
    return false;
  }

  // Execute trajectory
  moveit_msgs::RobotTrajectory robot_trajectory_msg;
  robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg);
  if (blocking)
  {
    trajectory_execution_manager_->push(robot_trajectory_msg);
    trajectory_execution_manager_->execute();
    return trajectory_execution_manager_->waitForExecution();
  }
  trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg);
  return true;
}

const std::shared_ptr<tf2_ros::Buffer>& MoveItCpp::getTFBuffer() const
{
  return tf_buffer_;
}

void MoveItCpp::clearContents()
{
  tf_listener_.reset();
  tf_buffer_.reset();
  planning_scene_monitor_.reset();
  robot_model_.reset();
  planning_pipelines_.clear();
}
}  // namespace moveit_cpp
<commit_msg>MoveItCpp: Allow multiple pipelines (#3131)<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, PickNik LLC
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of PickNik LLC nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Henning Kayser */

#include <stdexcept>
#include <moveit/moveit_cpp/moveit_cpp.h>

namespace moveit_cpp
{
constexpr char LOGNAME[] = "moveit_cpp";
constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin";

MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
  : MoveItCpp(Options(nh), nh, tf_buffer)
{
}

MoveItCpp::MoveItCpp(const Options& options, const ros::NodeHandle& nh,
                     const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
  : tf_buffer_(tf_buffer), node_handle_(nh)
{
  if (!tf_buffer_)
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>();
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

  // Configure planning scene monitor
  if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options))
  {
    const std::string error = "Unable to configure planning scene monitor";
    ROS_FATAL_STREAM_NAMED(LOGNAME, error);
    throw std::runtime_error(error);
  }

  robot_model_ = planning_scene_monitor_->getRobotModel();
  if (!robot_model_)
  {
    const std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
                              "parameter server.";
    ROS_FATAL_STREAM_NAMED(LOGNAME, error);
    throw std::runtime_error(error);
  }

  bool load_planning_pipelines = true;
  if (load_planning_pipelines && !loadPlanningPipelines(options.planning_pipeline_options))
  {
    const std::string error = "Failed to load planning pipelines from parameter server";
    ROS_FATAL_STREAM_NAMED(LOGNAME, error);
    throw std::runtime_error(error);
  }

  // TODO(henningkayser): configure trajectory execution manager
  trajectory_execution_manager_ = std::make_shared<trajectory_execution_manager::TrajectoryExecutionManager>(
      robot_model_, planning_scene_monitor_->getStateMonitor());

  ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp running");
}

MoveItCpp::~MoveItCpp()
{
  ROS_INFO_NAMED(LOGNAME, "Deleting MoveItCpp");
  clearContents();
}

bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options)
{
  planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(options.robot_description,
                                                                                tf_buffer_, options.name);
  // Allows us to sycronize to Rviz and also publish collision objects to ourselves
  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor");
  if (planning_scene_monitor_->getPlanningScene())
  {
    // Start state and scene monitors
    ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", options.joint_state_topic.c_str());
    // Subscribe to JointState sensor messages
    planning_scene_monitor_->startStateMonitor(options.joint_state_topic, options.attached_collision_object_topic);
    // Publish planning scene updates to remote monitors like RViz
    planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                          options.monitored_planning_scene_topic);
    // Monitor and apply planning scene updates from remote publishers like the PlanningSceneInterface
    planning_scene_monitor_->startSceneMonitor(options.publish_planning_scene_topic);
    // Monitor requests for changes in the collision environment
    planning_scene_monitor_->startWorldGeometryMonitor();
  }
  else
  {
    ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured");
    return false;
  }

  // Wait for complete state to be recieved
  if (options.wait_for_initial_state_timeout > 0.0)
  {
    return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(),
                                                                           options.wait_for_initial_state_timeout);
  }

  return true;
}

bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
{
  ros::NodeHandle node_handle(options.parent_namespace.empty() ? "~" : options.parent_namespace);
  for (const auto& planning_pipeline_name : options.pipeline_names)
  {
    if (planning_pipelines_.count(planning_pipeline_name) > 0)
    {
      ROS_WARN_NAMED(LOGNAME, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str());
      continue;
    }
    ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str());
    ros::NodeHandle child_nh(node_handle, planning_pipeline_name);
    planning_pipeline::PlanningPipelinePtr pipeline;
    pipeline = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, child_nh, PLANNING_PLUGIN_PARAM);

    if (!pipeline->getPlannerManager())
    {
      ROS_ERROR_NAMED(LOGNAME, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str());
      continue;
    }

    planning_pipelines_[planning_pipeline_name] = pipeline;
  }

  if (planning_pipelines_.empty())
  {
    ROS_ERROR_NAMED(LOGNAME, "Failed to load any planning pipelines.");
    return false;
  }

  // Retrieve group/pipeline mapping for faster lookup
  std::vector<std::string> group_names = robot_model_->getJointModelGroupNames();
  for (const auto& pipeline_entry : planning_pipelines_)
  {
    for (const auto& group_name : group_names)
    {
      const auto& pipeline = pipeline_entry.second;
      for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations())
      {
        if (planner_configuration.second.group == group_name)
        {
          groups_pipelines_map_[group_name].insert(pipeline_entry.first);
        }
      }
    }
  }

  return true;
}

moveit::core::RobotModelConstPtr MoveItCpp::getRobotModel() const
{
  return robot_model_;
}

const ros::NodeHandle& MoveItCpp::getNodeHandle() const
{
  return node_handle_;
}

bool MoveItCpp::getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds)
{
  if (wait_seconds > 0.0 &&
      !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds))
  {
    ROS_ERROR_NAMED(LOGNAME, "Did not receive robot state");
    return false;
  }
  {  // Lock planning scene
    planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor_);
    current_state = std::make_shared<moveit::core::RobotState>(scene->getCurrentState());
  }  // Unlock planning scene
  return true;
}

moveit::core::RobotStatePtr MoveItCpp::getCurrentState(double wait)
{
  moveit::core::RobotStatePtr current_state;
  getCurrentState(current_state, wait);
  return current_state;
}

const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::getPlanningPipelines() const
{
  return planning_pipelines_;
}

std::set<std::string> MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const
{
  if (group_name.empty() || groups_pipelines_map_.count(group_name) == 0)
  {
    ROS_ERROR_NAMED(LOGNAME,
                    "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
                    group_name.c_str());
    return {};  // empty
  }

  return groups_pipelines_map_.at(group_name);
}

const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const
{
  return planning_scene_monitor_;
}

planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonitorNonConst()
{
  return planning_scene_monitor_;
}

const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveItCpp::getTrajectoryExecutionManager() const
{
  return trajectory_execution_manager_;
}

trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajectoryExecutionManagerNonConst()
{
  return trajectory_execution_manager_;
}

bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
                        bool blocking)
{
  if (!robot_trajectory)
  {
    ROS_ERROR_NAMED(LOGNAME, "Robot trajectory is undefined");
    return false;
  }

  // Check if there are controllers that can handle the execution
  if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name))
  {
    ROS_ERROR_NAMED(LOGNAME, "Execution failed! No active controllers configured for group '%s'", group_name.c_str());
    return false;
  }

  // Execute trajectory
  moveit_msgs::RobotTrajectory robot_trajectory_msg;
  robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg);
  if (blocking)
  {
    trajectory_execution_manager_->push(robot_trajectory_msg);
    trajectory_execution_manager_->execute();
    return trajectory_execution_manager_->waitForExecution();
  }
  trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg);
  return true;
}

const std::shared_ptr<tf2_ros::Buffer>& MoveItCpp::getTFBuffer() const
{
  return tf_buffer_;
}

void MoveItCpp::clearContents()
{
  tf_listener_.reset();
  tf_buffer_.reset();
  planning_scene_monitor_.reset();
  robot_model_.reset();
  planning_pipelines_.clear();
}
}  // namespace moveit_cpp
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2010-2013 ARM Limited
 * All rights reserved
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * Copyright (c) 2002-2006 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Ali Saidi
 */

#include "arch/arm/linux/atag.hh"
#include "arch/arm/linux/system.hh"
#include "arch/arm/isa_traits.hh"
#include "arch/arm/utility.hh"
#include "arch/generic/linux/threadinfo.hh"
#include "base/loader/dtb_object.hh"
#include "base/loader/object_file.hh"
#include "base/loader/symtab.hh"
#include "cpu/base.hh"
#include "cpu/pc_event.hh"
#include "cpu/thread_context.hh"
#include "debug/Loader.hh"
#include "kern/linux/events.hh"
#include "mem/fs_translating_port_proxy.hh"
#include "mem/physical.hh"
#include "sim/stat_control.hh"

using namespace ArmISA;
using namespace Linux;

LinuxArmSystem::LinuxArmSystem(Params *p)
    : GenericArmSystem(p), dumpStatsPCEvent(nullptr),
      enableContextSwitchStatsDump(p->enable_context_switch_stats_dump),
      taskFile(nullptr), kernelPanicEvent(nullptr), kernelOopsEvent(nullptr)
{
    if (p->panic_on_panic) {
        kernelPanicEvent = addKernelFuncEventOrPanic<PanicPCEvent>(
            "panic", "Kernel panic in simulated kernel");
    } else {
#ifndef NDEBUG
        kernelPanicEvent = addKernelFuncEventOrPanic<BreakPCEvent>("panic");
#endif
    }

    if (p->panic_on_oops) {
        kernelOopsEvent = addKernelFuncEventOrPanic<PanicPCEvent>(
            "oops_exit", "Kernel oops in guest");
    }

    // With ARM udelay() is #defined to __udelay
    // newer kernels use __loop_udelay and __loop_const_udelay symbols
    uDelaySkipEvent = addKernelFuncEvent<UDelayEvent>(
        "__loop_udelay", "__udelay", 1000, 0);
    if (!uDelaySkipEvent)
        uDelaySkipEvent = addKernelFuncEventOrPanic<UDelayEvent>(
         "__udelay", "__udelay", 1000, 0);

    // constant arguments to udelay() have some precomputation done ahead of
    // time. Constant comes from code.
    constUDelaySkipEvent = addKernelFuncEvent<UDelayEvent>(
        "__loop_const_udelay", "__const_udelay", 1000, 107374);
    if (!constUDelaySkipEvent)
        constUDelaySkipEvent = addKernelFuncEventOrPanic<UDelayEvent>(
         "__const_udelay", "__const_udelay", 1000, 107374);

}

void
LinuxArmSystem::initState()
{
    // Moved from the constructor to here since it relies on the
    // address map being resolved in the interconnect

    // Call the initialisation of the super class
    GenericArmSystem::initState();

    // Load symbols at physical address, we might not want
    // to do this permanently, for but early bootup work
    // it is helpful.
    if (params()->early_kernel_symbols) {
        kernel->loadGlobalSymbols(kernelSymtab, 0, 0, loadAddrMask);
        kernel->loadGlobalSymbols(debugSymbolTable, 0, 0, loadAddrMask);
    }

    // Setup boot data structure
    Addr addr = 0;
    // Check if the kernel image has a symbol that tells us it supports
    // device trees.
    bool kernel_has_fdt_support =
        kernelSymtab->findAddress("unflatten_device_tree", addr);
    bool dtb_file_specified = params()->dtb_filename != "";

    if (kernel_has_fdt_support && dtb_file_specified) {
        // Kernel supports flattened device tree and dtb file specified.
        // Using Device Tree Blob to describe system configuration.
        inform("Loading DTB file: %s at address %#x\n", params()->dtb_filename,
                params()->atags_addr + loadAddrOffset);

        ObjectFile *dtb_file = createObjectFile(params()->dtb_filename, true);
        if (!dtb_file) {
            fatal("couldn't load DTB file: %s\n", params()->dtb_filename);
        }

        DtbObject *_dtb_file = dynamic_cast<DtbObject*>(dtb_file);

        if (_dtb_file) {
            if (!_dtb_file->addBootCmdLine(params()->boot_osflags.c_str(),
                                           params()->boot_osflags.size())) {
                warn("couldn't append bootargs to DTB file: %s\n",
                     params()->dtb_filename);
            }
        } else {
            warn("dtb_file cast failed; couldn't append bootargs "
                 "to DTB file: %s\n", params()->dtb_filename);
        }

        dtb_file->setTextBase(params()->atags_addr + loadAddrOffset);
        dtb_file->loadSections(physProxy);
        delete dtb_file;
    } else {
        // Using ATAGS
        // Warn if the kernel supports FDT and we haven't specified one
        if (kernel_has_fdt_support) {
            assert(!dtb_file_specified);
            warn("Kernel supports device tree, but no DTB file specified\n");
        }
        // Warn if the kernel doesn't support FDT and we have specified one
        if (dtb_file_specified) {
            assert(!kernel_has_fdt_support);
            warn("DTB file specified, but no device tree support in kernel\n");
        }

        AtagCore ac;
        ac.flags(1); // read-only
        ac.pagesize(8192);
        ac.rootdev(0);

        AddrRangeList atagRanges = physmem.getConfAddrRanges();
        if (atagRanges.size() != 1) {
            fatal("Expected a single ATAG memory entry but got %d\n",
                  atagRanges.size());
        }
        AtagMem am;
        am.memSize(atagRanges.begin()->size());
        am.memStart(atagRanges.begin()->start());

        AtagCmdline ad;
        ad.cmdline(params()->boot_osflags);

        DPRINTF(Loader, "boot command line %d bytes: %s\n",
                ad.size() <<2, params()->boot_osflags.c_str());

        AtagNone an;

        uint32_t size = ac.size() + am.size() + ad.size() + an.size();
        uint32_t offset = 0;
        uint8_t *boot_data = new uint8_t[size << 2];

        offset += ac.copyOut(boot_data + offset);
        offset += am.copyOut(boot_data + offset);
        offset += ad.copyOut(boot_data + offset);
        offset += an.copyOut(boot_data + offset);

        DPRINTF(Loader, "Boot atags was %d bytes in total\n", size << 2);
        DDUMP(Loader, boot_data, size << 2);

        physProxy.writeBlob(params()->atags_addr + loadAddrOffset, boot_data,
                size << 2);

        delete[] boot_data;
    }

    // Kernel boot requirements to set up r0, r1 and r2 in ARMv7
    for (int i = 0; i < threadContexts.size(); i++) {
        threadContexts[i]->setIntReg(0, 0);
        threadContexts[i]->setIntReg(1, params()->machine_type);
        threadContexts[i]->setIntReg(2, params()->atags_addr + loadAddrOffset);
    }
}

LinuxArmSystem::~LinuxArmSystem()
{
    if (uDelaySkipEvent)
        delete uDelaySkipEvent;
    if (constUDelaySkipEvent)
        delete constUDelaySkipEvent;

    if (dumpStatsPCEvent)
        delete dumpStatsPCEvent;
}

LinuxArmSystem *
LinuxArmSystemParams::create()
{
    return new LinuxArmSystem(this);
}

void
LinuxArmSystem::startup()
{
    if (enableContextSwitchStatsDump) {
        dumpStatsPCEvent = addKernelFuncEvent<DumpStatsPCEvent>("__switch_to");
        if (!dumpStatsPCEvent)
           panic("dumpStatsPCEvent not created!");

        std::string task_filename = "tasks.txt";
        taskFile = simout.create(name() + "." + task_filename);

        for (int i = 0; i < _numContexts; i++) {
            ThreadContext *tc = threadContexts[i];
            uint32_t pid = tc->getCpuPtr()->getPid();
            if (pid != BaseCPU::invldPid) {
                mapPid(tc, pid);
                tc->getCpuPtr()->taskId(taskMap[pid]);
            }
        }
    }
}

void
LinuxArmSystem::mapPid(ThreadContext *tc, uint32_t pid)
{
    // Create a new unique identifier for this pid
    std::map<uint32_t, uint32_t>::iterator itr = taskMap.find(pid);
    if (itr == taskMap.end()) {
        uint32_t map_size = taskMap.size();
        if (map_size > ContextSwitchTaskId::MaxNormalTaskId + 1) {
            warn_once("Error out of identifiers for cache occupancy stats");
            taskMap[pid] = ContextSwitchTaskId::Unknown;
        } else {
            taskMap[pid] = map_size;
        }
    }
}

/** This function is called whenever the the kernel function
 *  "__switch_to" is called to change running tasks.
 *
 *  r0 = task_struct of the previously running process
 *  r1 = task_info of the previously running process
 *  r2 = task_info of the next process to run
 */
void
DumpStatsPCEvent::process(ThreadContext *tc)
{
    Linux::ThreadInfo ti(tc);
    Addr task_descriptor = tc->readIntReg(2);
    uint32_t pid = ti.curTaskPID(task_descriptor);
    uint32_t tgid = ti.curTaskTGID(task_descriptor);
    std::string next_task_str = ti.curTaskName(task_descriptor);

    // Streamline treats pid == -1 as the kernel process.
    // Also pid == 0 implies idle process (except during Linux boot)
    int32_t mm = ti.curTaskMm(task_descriptor);
    bool is_kernel = (mm == 0);
    if (is_kernel && (pid != 0)) {
        pid = -1;
        tgid = -1;
        next_task_str = "kernel";
    }

    LinuxArmSystem* sys = dynamic_cast<LinuxArmSystem *>(tc->getSystemPtr());
    if (!sys) {
        panic("System is not LinuxArmSystem while getting Linux process info!");
    }
    std::map<uint32_t, uint32_t>& taskMap = sys->taskMap;

    // Create a new unique identifier for this pid
    sys->mapPid(tc, pid);

    // Set cpu task id, output process info, and dump stats
    tc->getCpuPtr()->taskId(taskMap[pid]);
    tc->getCpuPtr()->setPid(pid);

    OutputStream* taskFile = sys->taskFile;

    // Task file is read by cache occupancy plotting script or
    // Streamline conversion script.
    ccprintf(*(taskFile->stream()),
             "tick=%lld %d cpu_id=%d next_pid=%d next_tgid=%d next_task=%s\n",
             curTick(), taskMap[pid], tc->cpuId(), (int) pid, (int) tgid,
             next_task_str);
    taskFile->stream()->flush();

    // Dump and reset statistics
    Stats::schedStatEvent(true, true, curTick(), 0);
}

<commit_msg>arm: Remove BreakPCEvent on guest kernel panic<commit_after>/*
 * Copyright (c) 2010-2013 ARM Limited
 * All rights reserved
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * Copyright (c) 2002-2006 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Ali Saidi
 */

#include "arch/arm/linux/atag.hh"
#include "arch/arm/linux/system.hh"
#include "arch/arm/isa_traits.hh"
#include "arch/arm/utility.hh"
#include "arch/generic/linux/threadinfo.hh"
#include "base/loader/dtb_object.hh"
#include "base/loader/object_file.hh"
#include "base/loader/symtab.hh"
#include "cpu/base.hh"
#include "cpu/pc_event.hh"
#include "cpu/thread_context.hh"
#include "debug/Loader.hh"
#include "kern/linux/events.hh"
#include "mem/fs_translating_port_proxy.hh"
#include "mem/physical.hh"
#include "sim/stat_control.hh"

using namespace ArmISA;
using namespace Linux;

LinuxArmSystem::LinuxArmSystem(Params *p)
    : GenericArmSystem(p), dumpStatsPCEvent(nullptr),
      enableContextSwitchStatsDump(p->enable_context_switch_stats_dump),
      taskFile(nullptr), kernelPanicEvent(nullptr), kernelOopsEvent(nullptr)
{
    if (p->panic_on_panic) {
        kernelPanicEvent = addKernelFuncEventOrPanic<PanicPCEvent>(
            "panic", "Kernel panic in simulated kernel");
    }

    if (p->panic_on_oops) {
        kernelOopsEvent = addKernelFuncEventOrPanic<PanicPCEvent>(
            "oops_exit", "Kernel oops in guest");
    }

    // With ARM udelay() is #defined to __udelay
    // newer kernels use __loop_udelay and __loop_const_udelay symbols
    uDelaySkipEvent = addKernelFuncEvent<UDelayEvent>(
        "__loop_udelay", "__udelay", 1000, 0);
    if (!uDelaySkipEvent)
        uDelaySkipEvent = addKernelFuncEventOrPanic<UDelayEvent>(
         "__udelay", "__udelay", 1000, 0);

    // constant arguments to udelay() have some precomputation done ahead of
    // time. Constant comes from code.
    constUDelaySkipEvent = addKernelFuncEvent<UDelayEvent>(
        "__loop_const_udelay", "__const_udelay", 1000, 107374);
    if (!constUDelaySkipEvent)
        constUDelaySkipEvent = addKernelFuncEventOrPanic<UDelayEvent>(
         "__const_udelay", "__const_udelay", 1000, 107374);

}

void
LinuxArmSystem::initState()
{
    // Moved from the constructor to here since it relies on the
    // address map being resolved in the interconnect

    // Call the initialisation of the super class
    GenericArmSystem::initState();

    // Load symbols at physical address, we might not want
    // to do this permanently, for but early bootup work
    // it is helpful.
    if (params()->early_kernel_symbols) {
        kernel->loadGlobalSymbols(kernelSymtab, 0, 0, loadAddrMask);
        kernel->loadGlobalSymbols(debugSymbolTable, 0, 0, loadAddrMask);
    }

    // Setup boot data structure
    Addr addr = 0;
    // Check if the kernel image has a symbol that tells us it supports
    // device trees.
    bool kernel_has_fdt_support =
        kernelSymtab->findAddress("unflatten_device_tree", addr);
    bool dtb_file_specified = params()->dtb_filename != "";

    if (kernel_has_fdt_support && dtb_file_specified) {
        // Kernel supports flattened device tree and dtb file specified.
        // Using Device Tree Blob to describe system configuration.
        inform("Loading DTB file: %s at address %#x\n", params()->dtb_filename,
                params()->atags_addr + loadAddrOffset);

        ObjectFile *dtb_file = createObjectFile(params()->dtb_filename, true);
        if (!dtb_file) {
            fatal("couldn't load DTB file: %s\n", params()->dtb_filename);
        }

        DtbObject *_dtb_file = dynamic_cast<DtbObject*>(dtb_file);

        if (_dtb_file) {
            if (!_dtb_file->addBootCmdLine(params()->boot_osflags.c_str(),
                                           params()->boot_osflags.size())) {
                warn("couldn't append bootargs to DTB file: %s\n",
                     params()->dtb_filename);
            }
        } else {
            warn("dtb_file cast failed; couldn't append bootargs "
                 "to DTB file: %s\n", params()->dtb_filename);
        }

        dtb_file->setTextBase(params()->atags_addr + loadAddrOffset);
        dtb_file->loadSections(physProxy);
        delete dtb_file;
    } else {
        // Using ATAGS
        // Warn if the kernel supports FDT and we haven't specified one
        if (kernel_has_fdt_support) {
            assert(!dtb_file_specified);
            warn("Kernel supports device tree, but no DTB file specified\n");
        }
        // Warn if the kernel doesn't support FDT and we have specified one
        if (dtb_file_specified) {
            assert(!kernel_has_fdt_support);
            warn("DTB file specified, but no device tree support in kernel\n");
        }

        AtagCore ac;
        ac.flags(1); // read-only
        ac.pagesize(8192);
        ac.rootdev(0);

        AddrRangeList atagRanges = physmem.getConfAddrRanges();
        if (atagRanges.size() != 1) {
            fatal("Expected a single ATAG memory entry but got %d\n",
                  atagRanges.size());
        }
        AtagMem am;
        am.memSize(atagRanges.begin()->size());
        am.memStart(atagRanges.begin()->start());

        AtagCmdline ad;
        ad.cmdline(params()->boot_osflags);

        DPRINTF(Loader, "boot command line %d bytes: %s\n",
                ad.size() <<2, params()->boot_osflags.c_str());

        AtagNone an;

        uint32_t size = ac.size() + am.size() + ad.size() + an.size();
        uint32_t offset = 0;
        uint8_t *boot_data = new uint8_t[size << 2];

        offset += ac.copyOut(boot_data + offset);
        offset += am.copyOut(boot_data + offset);
        offset += ad.copyOut(boot_data + offset);
        offset += an.copyOut(boot_data + offset);

        DPRINTF(Loader, "Boot atags was %d bytes in total\n", size << 2);
        DDUMP(Loader, boot_data, size << 2);

        physProxy.writeBlob(params()->atags_addr + loadAddrOffset, boot_data,
                size << 2);

        delete[] boot_data;
    }

    // Kernel boot requirements to set up r0, r1 and r2 in ARMv7
    for (int i = 0; i < threadContexts.size(); i++) {
        threadContexts[i]->setIntReg(0, 0);
        threadContexts[i]->setIntReg(1, params()->machine_type);
        threadContexts[i]->setIntReg(2, params()->atags_addr + loadAddrOffset);
    }
}

LinuxArmSystem::~LinuxArmSystem()
{
    if (uDelaySkipEvent)
        delete uDelaySkipEvent;
    if (constUDelaySkipEvent)
        delete constUDelaySkipEvent;

    if (dumpStatsPCEvent)
        delete dumpStatsPCEvent;
}

LinuxArmSystem *
LinuxArmSystemParams::create()
{
    return new LinuxArmSystem(this);
}

void
LinuxArmSystem::startup()
{
    if (enableContextSwitchStatsDump) {
        dumpStatsPCEvent = addKernelFuncEvent<DumpStatsPCEvent>("__switch_to");
        if (!dumpStatsPCEvent)
           panic("dumpStatsPCEvent not created!");

        std::string task_filename = "tasks.txt";
        taskFile = simout.create(name() + "." + task_filename);

        for (int i = 0; i < _numContexts; i++) {
            ThreadContext *tc = threadContexts[i];
            uint32_t pid = tc->getCpuPtr()->getPid();
            if (pid != BaseCPU::invldPid) {
                mapPid(tc, pid);
                tc->getCpuPtr()->taskId(taskMap[pid]);
            }
        }
    }
}

void
LinuxArmSystem::mapPid(ThreadContext *tc, uint32_t pid)
{
    // Create a new unique identifier for this pid
    std::map<uint32_t, uint32_t>::iterator itr = taskMap.find(pid);
    if (itr == taskMap.end()) {
        uint32_t map_size = taskMap.size();
        if (map_size > ContextSwitchTaskId::MaxNormalTaskId + 1) {
            warn_once("Error out of identifiers for cache occupancy stats");
            taskMap[pid] = ContextSwitchTaskId::Unknown;
        } else {
            taskMap[pid] = map_size;
        }
    }
}

/** This function is called whenever the the kernel function
 *  "__switch_to" is called to change running tasks.
 *
 *  r0 = task_struct of the previously running process
 *  r1 = task_info of the previously running process
 *  r2 = task_info of the next process to run
 */
void
DumpStatsPCEvent::process(ThreadContext *tc)
{
    Linux::ThreadInfo ti(tc);
    Addr task_descriptor = tc->readIntReg(2);
    uint32_t pid = ti.curTaskPID(task_descriptor);
    uint32_t tgid = ti.curTaskTGID(task_descriptor);
    std::string next_task_str = ti.curTaskName(task_descriptor);

    // Streamline treats pid == -1 as the kernel process.
    // Also pid == 0 implies idle process (except during Linux boot)
    int32_t mm = ti.curTaskMm(task_descriptor);
    bool is_kernel = (mm == 0);
    if (is_kernel && (pid != 0)) {
        pid = -1;
        tgid = -1;
        next_task_str = "kernel";
    }

    LinuxArmSystem* sys = dynamic_cast<LinuxArmSystem *>(tc->getSystemPtr());
    if (!sys) {
        panic("System is not LinuxArmSystem while getting Linux process info!");
    }
    std::map<uint32_t, uint32_t>& taskMap = sys->taskMap;

    // Create a new unique identifier for this pid
    sys->mapPid(tc, pid);

    // Set cpu task id, output process info, and dump stats
    tc->getCpuPtr()->taskId(taskMap[pid]);
    tc->getCpuPtr()->setPid(pid);

    OutputStream* taskFile = sys->taskFile;

    // Task file is read by cache occupancy plotting script or
    // Streamline conversion script.
    ccprintf(*(taskFile->stream()),
             "tick=%lld %d cpu_id=%d next_pid=%d next_tgid=%d next_task=%s\n",
             curTick(), taskMap[pid], tc->cpuId(), (int) pid, (int) tgid,
             next_task_str);
    taskFile->stream()->flush();

    // Dump and reset statistics
    Stats::schedStatEvent(true, true, curTick(), 0);
}

<|endoftext|>


<commit_before>/*
 * Copyright (c) 2003-2005 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Gabe Black
 *          Ali Saidi
 */

#ifndef __ARCH_SPARC_INTREGFILE_HH__
#define __ARCH_SPARC_INTREGFILE_HH__

#include "arch/sparc/isa_traits.hh"
#include "arch/sparc/types.hh"
#include "base/bitfield.hh"

#include <string>

class Checkpoint;

namespace SparcISA
{
    class RegFile;

    //This function translates integer register file indices into names
    std::string getIntRegName(RegIndex);

    const int NumIntArchRegs = 32;
    const int NumIntRegs = (MaxGL + 1) * 8 + NWindows * 16 + NumMicroIntRegs;

    class IntRegFile
    {
      private:
        friend class RegFile;
      protected:
        //The number of bits needed to index into each 8 register frame
        static const int FrameOffsetBits = 3;
        //The number of bits to choose between the 4 sets of 8 registers
        static const int FrameNumBits = 2;

        //The number of registers per "frame" (8)
        static const int RegsPerFrame = 1 << FrameOffsetBits;
        //A mask to get the frame number
        static const uint64_t FrameNumMask =
                (FrameNumBits == sizeof(int)) ?
                (unsigned int)(-1) :
                (1 << FrameNumBits) - 1;
        static const uint64_t FrameOffsetMask =
                (FrameOffsetBits == sizeof(int)) ?
                (unsigned int)(-1) :
                (1 << FrameOffsetBits) - 1;

        IntReg regGlobals[MaxGL+1][RegsPerFrame];
        IntReg regSegments[2 * NWindows][RegsPerFrame];
        IntReg microRegs[NumMicroIntRegs];
        IntReg regs[NumIntRegs];

        enum regFrame {Globals, Outputs, Locals, Inputs, NumFrames};

        IntReg * regView[NumFrames];

        static const int RegGlobalOffset = 0;
        static const int FrameOffset = MaxGL * RegsPerFrame;
        int offset[NumFrames];

      public:

        int flattenIndex(int reg);

        void clear();

        IntRegFile();

        IntReg readReg(int intReg);

        void setReg(int intReg, const IntReg &val);

        void serialize(std::ostream &os);

        void unserialize(Checkpoint *cp, const std::string &section);

      protected:
        //This doesn't effect the actual CWP register.
        //It's purpose is to adjust the view of the register file
        //to what it would be if CWP = cwp.
        void setCWP(int cwp);

        void setGlobals(int gl);
    };
}

#endif
<commit_msg>Fixed an off-by-one error.<commit_after>/*
 * Copyright (c) 2003-2005 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Gabe Black
 *          Ali Saidi
 */

#ifndef __ARCH_SPARC_INTREGFILE_HH__
#define __ARCH_SPARC_INTREGFILE_HH__

#include "arch/sparc/isa_traits.hh"
#include "arch/sparc/types.hh"
#include "base/bitfield.hh"

#include <string>

class Checkpoint;

namespace SparcISA
{
    class RegFile;

    //This function translates integer register file indices into names
    std::string getIntRegName(RegIndex);

    const int NumIntArchRegs = 32;
    const int NumIntRegs = (MaxGL + 1) * 8 + NWindows * 16 + NumMicroIntRegs;

    class IntRegFile
    {
      private:
        friend class RegFile;
      protected:
        //The number of bits needed to index into each 8 register frame
        static const int FrameOffsetBits = 3;
        //The number of bits to choose between the 4 sets of 8 registers
        static const int FrameNumBits = 2;

        //The number of registers per "frame" (8)
        static const int RegsPerFrame = 1 << FrameOffsetBits;
        //A mask to get the frame number
        static const uint64_t FrameNumMask =
                (FrameNumBits == sizeof(int)) ?
                (unsigned int)(-1) :
                (1 << FrameNumBits) - 1;
        static const uint64_t FrameOffsetMask =
                (FrameOffsetBits == sizeof(int)) ?
                (unsigned int)(-1) :
                (1 << FrameOffsetBits) - 1;

        IntReg regGlobals[MaxGL+1][RegsPerFrame];
        IntReg regSegments[2 * NWindows][RegsPerFrame];
        IntReg microRegs[NumMicroIntRegs];
        IntReg regs[NumIntRegs];

        enum regFrame {Globals, Outputs, Locals, Inputs, NumFrames};

        IntReg * regView[NumFrames];

        static const int RegGlobalOffset = 0;
        static const int FrameOffset = (MaxGL + 1) * RegsPerFrame;
        int offset[NumFrames];

      public:

        int flattenIndex(int reg);

        void clear();

        IntRegFile();

        IntReg readReg(int intReg);

        void setReg(int intReg, const IntReg &val);

        void serialize(std::ostream &os);

        void unserialize(Checkpoint *cp, const std::string &section);

      protected:
        //This doesn't effect the actual CWP register.
        //It's purpose is to adjust the view of the register file
        //to what it would be if CWP = cwp.
        void setCWP(int cwp);

        void setGlobals(int gl);
    };
}

#endif
<|endoftext|>


<commit_before>/**
 * @file keyboard.cpp
 * @date 28.06.2013
 * @author Denise Ratasich
 *
 * @brief ROS node reading from stdin to steer the robot via keyboard.
 *
 * A new speed is only published when a key is pressed. Note reading
 * vom stdin (console) is blocking.
 *
 * - subscribe: none
 * - publish: ~cmd_vel [geometry_msgs/Twist]
 *
 * parameters
 * - ~pub_period_ms: The publishing period of cmd_vel in ms. Default
 *   is set to 500ms.
 */

// c includes (for terminal, threading)
#include <stdio.h>
#include <termios.h>    //termios, TCSANOW, ECHO, ICANON
#include <unistd.h>     //STDIN_FILENO
#include <boost/thread.hpp>

// ROS includes
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

// Pioneer includes
#include "pioneer3.hpp"

using namespace std;

#define MSG_BUFFER_SIZE			1

// Parameters.
int pub_period_ms = 500;

/**
 * @brief Guards a twist object and an additional running flag for the
 * keyboard thread, i.e., ensures mutual exclusion of the variables to
 * exchange.
 */
class MutexedData {
  boost::mutex mtx_;
  geometry_msgs::Twist twist_;
  int running_;
public:
  MutexedData() {
    twist_.linear.x = 0;
    twist_.angular.z = 0;
    running_ = 0;
  }
  void setTwist(geometry_msgs::Twist& twist) {
    mtx_.lock();
    twist_.linear.x = twist.linear.x;
    twist_.angular.z = twist.angular.z;
    mtx_.unlock();
  }
  void setTwist(float lin, float ang) {
    mtx_.lock();
    twist_.linear.x = lin;
    twist_.angular.z = ang;
    mtx_.unlock();
  }
  geometry_msgs::Twist getTwist() {
    geometry_msgs::Twist twist;
    mtx_.lock();
    twist.linear.x = twist_.linear.x;
    twist.angular.z = twist_.angular.z;
    mtx_.unlock();
    return twist;
  }
  void setRunning(int running) {
    mtx_.lock();
    running_ = running;
    mtx_.unlock();
  }
  int getRunning() {
    int r;
    mtx_.lock();
    r = running_;
    mtx_.unlock();
    return r;
  }
};

// Instantiate the guarded twist object.
MutexedData mtxData;

// Forward declarations.
void applyParameters(ros::NodeHandle& n);
void getTwistFromKeyboard();

/**
 * This node publishes to cmd_vel to steer the robot according to a
 * key pressed in the terminal window this application is running.
 */
int main(int argc, char **argv)
{
  static struct termios oldt, newt;

  // Disable buffering (till ENTER is pressed) and echo of terminal.
  tcgetattr( STDIN_FILENO, &oldt);		// get the parameters of the current terminal
  newt = oldt;
  newt.c_lflag &= ~(ICANON | ECHO);
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);	// change attributes immediately

  // Initialize for ROS.
  ros::init(argc, argv, "pioneer_teleop");
  if (ros::this_node::getNamespace() == "/")
    ROS_WARN("Started in the global namespace.");

  // Create main access point to communicate with the ROS system.
  ros::NodeHandle n("~");

  // Apply parameters from parameter server.
  applyParameters(n);

  // Note, how to terminate this node!
  ROS_WARN("This node can only be terminated by pressing 'q'.");

  // Tell ROS that we want to publish on the topic cmd_vel (for steering the robot).
  ros::Publisher pub_cmdVel = n.advertise < geometry_msgs::Twist > ("cmd_vel", MSG_BUFFER_SIZE);

  // Wait a little bit to be sure that the connection is established.
  ros::Duration duration(0.5);
  duration.sleep();
  duration.sec = 0;
  duration.nsec = pub_period_ms * 1e6;

  // Start thread to read from keyboard, updates mtxTwist if a key is
  // pressed.
  boost::thread keyboardThread(getTwistFromKeyboard);
  mtxData.setRunning(1);	// set running flag of the thread
  geometry_msgs::Twist twist;	// only local variable

  while (ros::ok() && mtxData.getRunning())
  {
    // Send message.
    twist = mtxData.getTwist();
    pub_cmdVel.publish(twist);
    ROS_DEBUG("%.2f m/s, %.2f degree", twist.linear.x, twist.angular.z);

    // Handle callbacks if any.
    ros::spinOnce();

    // Timeout.
    duration.sleep();
  }

  if (mtxData.getRunning())
    ROS_ERROR("Ctrl-C received. Press 'q' to quit the node!");
  keyboardThread.join();

  // Send "stop" for robot.
  twist.linear.x = 0;
  twist.angular.z = 0;
  pub_cmdVel.publish(twist);
  ROS_INFO("Robot stopped.");

  // Close connections.
  pub_cmdVel.shutdown();
  ROS_INFO("Closed connection.");

  // Restore old terminal settings.
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);

  return 0;
}

void applyParameters(ros::NodeHandle& n)
{
  // ROS params ---
  if (n.hasParam("pub_period_ms")) {
    n.getParam("pub_period_ms", pub_period_ms);
    ROS_INFO("Publishing period set to %d ms.", pub_period_ms);
  } else {
    ROS_INFO("Publishing period set to default %d ms.", pub_period_ms);
  }
}

/**
 * @brief Waits on a key pressed and sets the twist according to this
 * key.
 */
void getTwistFromKeyboard()
{
  float speed_lin = 0;
  float speed_ang = 0;
  char keyPressed;
  bool run = 1;

  while(run)
  {
    //cin >> keyPressed;		// cin cannot read space
    keyPressed = getchar();	// blocking!!!

    switch (keyPressed)
    {
    case ' ':	// space
      // stop
      speed_lin = 0;
      speed_ang = 0;
      ROS_INFO("stop");
      break;
    case 'a':
      // left
      if (speed_ang < SPEED_MAX)
	speed_ang += SPEED_STEP;
      break;
    case 'd':
      // right
      if (speed_ang > -SPEED_MAX)
	speed_ang -= SPEED_STEP;
      break;
    case 's':
      // backward
      if (speed_lin > -SPEED_MAX)
	speed_lin -= SPEED_STEP;

      if (speed_ang != 0) {
	speed_ang = 0;
	ROS_INFO("straight backward");
      }
      break;
    case 'w':
      // forward
      if (speed_lin < SPEED_MAX)
	speed_lin += SPEED_STEP;

      if (speed_ang != 0) {
	speed_ang = 0;
	ROS_INFO("straight forward");
      }
      break;
    case 'q':
      // quit
      ROS_INFO("quit");
      mtxData.setRunning(0);
      run = 0;			// leave loop and terminate this thread
    }

    mtxData.setTwist(speed_lin * SCALE_TRANSLATION, speed_ang * SCALE_ROTATION);
  }
}
<commit_msg>[pioneer_teleop] publish when key pressed (additionally to periodic publishing)<commit_after>/**
 * @file keyboard.cpp
 * @date 28.06.2013
 * @author Denise Ratasich
 *
 * @brief ROS node reading from stdin to steer the robot via keyboard.
 *
 * A new speed is only published when a key is pressed. Note reading
 * vom stdin (console) is blocking.
 *
 * - subscribe: none
 * - publish: ~cmd_vel [geometry_msgs/Twist]
 *
 * parameters
 * - ~pub_period_ms: The publishing period of cmd_vel in ms. Default
 *   is set to 500ms.
 */

// c includes (for terminal, threading)
#include <stdio.h>
#include <termios.h>    //termios, TCSANOW, ECHO, ICANON
#include <unistd.h>     //STDIN_FILENO
#include <boost/thread.hpp>

// ROS includes
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

// Pioneer includes
#include "pioneer3.hpp"

using namespace std;

#define MSG_BUFFER_SIZE			1

// Parameters.
int pub_period_ms = 500;

/**
 * @brief Guards a twist object and an additional running flag for the
 * keyboard thread, i.e., ensures mutual exclusion of the variables to
 * exchange.
 */
class MutexedData {
  boost::mutex mtx_;			// for mutual exclusion
  geometry_msgs::Twist twist_;		// the data to send
  ros::Publisher pub_cmdVel_;		// the publisher (thread is also able to publish)
  int running_;
public:
  void init(ros::NodeHandle& n) {
    twist_.linear.x = 0;
    twist_.angular.z = 0;
    pub_cmdVel_ = n.advertise < geometry_msgs::Twist > ("cmd_vel", MSG_BUFFER_SIZE);
    running_ = 0;
  }
  void shutdown() {
    pub_cmdVel_.shutdown();
  }
  void setTwist(geometry_msgs::Twist& twist) {
    mtx_.lock();
    twist_.linear.x = twist.linear.x;
    twist_.angular.z = twist.angular.z;
    mtx_.unlock();
  }
  void setTwist(float lin, float ang) {
    mtx_.lock();
    twist_.linear.x = lin;
    twist_.angular.z = ang;
    mtx_.unlock();
  }
  geometry_msgs::Twist getTwist() {
    geometry_msgs::Twist twist;
    mtx_.lock();
    twist.linear.x = twist_.linear.x;
    twist.angular.z = twist_.angular.z;
    mtx_.unlock();
    return twist;
  }
  void publish() {
    mtx_.lock();
    pub_cmdVel_.publish(twist_);
    ROS_DEBUG("%.2f m/s, %.2f degree", twist_.linear.x, twist_.angular.z);
    mtx_.unlock();
  }
  void publish(float lin, float ang) {
    mtx_.lock();
    twist_.linear.x = lin;
    twist_.angular.z = ang;
    pub_cmdVel_.publish(twist_);
    ROS_DEBUG("%.2f m/s, %.2f degree", twist_.linear.x, twist_.angular.z);
    mtx_.unlock();
  }
  void setRunning(int running) {
    mtx_.lock();
    running_ = running;
    mtx_.unlock();
  }
  int isRunning() {
    int r;
    mtx_.lock();
    r = running_;
    mtx_.unlock();
    return r;
  }
};

// Instantiate the guarded twist object.
MutexedData mtxData;

// Forward declarations.
void applyParameters(ros::NodeHandle& n);
void getTwistFromKeyboard();

/**
 * This node publishes to cmd_vel to steer the robot according to a
 * key pressed in the terminal window this application is running.
 */
int main(int argc, char **argv)
{
  static struct termios oldt, newt;

  // Disable buffering (till ENTER is pressed) and echo of terminal.
  tcgetattr( STDIN_FILENO, &oldt);		// get the parameters of the current terminal
  newt = oldt;
  newt.c_lflag &= ~(ICANON | ECHO);
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);	// change attributes immediately

  // Initialize for ROS.
  ros::init(argc, argv, "pioneer_teleop");
  if (ros::this_node::getNamespace() == "/")
    ROS_WARN("Started in the global namespace.");

  // Create main access point to communicate with the ROS system.
  ros::NodeHandle n("~");

  // Apply parameters from parameter server.
  applyParameters(n);

  // Note, how to terminate this node!
  ROS_WARN("This node can only be terminated by pressing 'q'.");

  // Initialize data and tell ROS that we want to publish on the topic
  // cmd_vel (for steering the robot).
  mtxData.init(n);

  // Wait a little bit to be sure that the connection is established.
  ros::Duration duration(0.5);
  duration.sleep();
  duration.sec = 0;
  duration.nsec = pub_period_ms * 1e6;

  // Start thread to read from keyboard, updates mtxData if a key is
  // pressed.
  boost::thread keyboardThread(getTwistFromKeyboard);
  mtxData.setRunning(1);	// set running flag of the thread

  while (ros::ok() && mtxData.isRunning())
  {
    // Send message periodically.
    mtxData.publish();

    // Handle callbacks if any.
    ros::spinOnce();

    // Timeout.
    duration.sleep();
  }

  if (mtxData.isRunning())
    ROS_ERROR("Ctrl-C received. Press 'q' to quit the node!");
  keyboardThread.join();

  // Send "stop" for robot.
  mtxData.publish(0,0);
  ROS_INFO("Robot stopped.");

  // Close connections.
  mtxData.shutdown();
  ROS_INFO("Closed connection.");

  // Restore old terminal settings.
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);

  return 0;
}

/**
 * @brief Fetches available params from the parameter server and
 * configures this node.
 */
void applyParameters(ros::NodeHandle& n)
{
  // ROS params ---
  if (n.hasParam("pub_period_ms")) {
    n.getParam("pub_period_ms", pub_period_ms);
    ROS_INFO("Publishing period set to %d ms.", pub_period_ms);
  } else {
    ROS_INFO("Publishing period set to default %d ms.", pub_period_ms);
  }
}

/**
 * @brief Waits on a key pressed and sets the twist according to this
 * key.
 */
void getTwistFromKeyboard()
{
  float speed_lin = 0;
  float speed_ang = 0;
  char keyPressed;
  bool run = 1;

  while(run)
  {
    //cin >> keyPressed;		// cin cannot read space
    keyPressed = getchar();	// blocking!!!

    switch (keyPressed)
    {
    case ' ':	// space
      // stop
      speed_lin = 0;
      speed_ang = 0;
      ROS_INFO("stop");
      break;
    case 'a':
      // left
      if (speed_ang < SPEED_MAX)
	speed_ang += SPEED_STEP;
      break;
    case 'd':
      // right
      if (speed_ang > -SPEED_MAX)
	speed_ang -= SPEED_STEP;
      break;
    case 's':
      // backward
      if (speed_lin > -SPEED_MAX)
	speed_lin -= SPEED_STEP;

      if (speed_ang != 0) {
	speed_ang = 0;
	ROS_INFO("straight backward");
      }
      break;
    case 'w':
      // forward
      if (speed_lin < SPEED_MAX)
	speed_lin += SPEED_STEP;

      if (speed_ang != 0) {
	speed_ang = 0;
	ROS_INFO("straight forward");
      }
      break;
    case 'q':
      // quit
      ROS_INFO("quit");
      mtxData.setRunning(0);
      run = 0;			// leave loop and terminate this thread
    }

    mtxData.publish(speed_lin * SCALE_TRANSLATION, speed_ang * SCALE_ROTATION);
  }
}
<|endoftext|>


<commit_before>#include "GameController.h"

#include "Tools/Debug/DebugRequest.h"
#include <PlatformInterface/Platform.h>

GameController::GameController()
  :
  lastWhistleCount(0),
  returnMessage(GameReturnData::alive)
{
  DEBUG_REQUEST_REGISTER("gamecontroller:play", "force the play state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:penalized", "force the penalized state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:initial", "force the initial state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:ready", "force the ready state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:set", "force the set state", false);

  // TODO: make it parameters?
  // load values from config
  const Configuration& config = naoth::Platform::getInstance().theConfiguration;

  if (config.hasKey("player", "NumOfPlayer")) {
    getPlayerInfo().playersPerTeam = config.getInt("player", "NumOfPlayer");
  } else {
    std::cerr << "[GameData] " << "No number of players (NumOfPlayers) given" << std::endl;
  }

  if (config.hasKey("player", "PlayerNumber")) {
    getPlayerInfo().playerNumber = config.getInt("player", "PlayerNumber");
  } else {
    std::cerr << "[PlayerInfo] " << "No player number (PlayerNumber) given" << std::endl;
    getPlayerInfo().playerNumber = 3;
  }

  if (config.hasKey("player", "TeamNumber")) {
    getPlayerInfo().teamNumber = config.getInt("player", "TeamNumber");
  } else {
    std::cerr << "[PlayerInfo] " << "No team number (TeamNumber) given" << std::endl;
    getPlayerInfo().teamNumber = 0;
  }

  if (config.hasKey("player", "TeamName")) {
    getPlayerInfo().teamName = config.getString("player", "TeamName");
  } else {
    std::cerr << "[PlayerInfo] " << "No team name (TeamName) given" << std::endl;
    getPlayerInfo().teamName = "unknown";
  }

  // NOTE: default team color is red
  getPlayerInfo().teamColor = GameData::red;
  if (config.hasKey("player", "TeamColor"))
  {
    getPlayerInfo().teamColor = GameData::teamColorFromString(config.getString("player", "TeamColor"));
    if (getPlayerInfo().teamColor == GameData::unknown_team_color)
    {
      std::cerr << "[GameData] " << "Invalid team color (TeamColor) \""
        << config.getString("player", "TeamColor") << "\" given" << std::endl;
    }
  } else {
    std::cerr << "[GameData] " << "No team color (TeamColor) given" << std::endl;
  }
}

void GameController::execute()
{
  PlayerInfo::RobotState oldRobotState = getPlayerInfo().robotState;
  GameData::TeamColor oldTeamColor = getPlayerInfo().teamColor;

  // try update from the game controller message
  if ( getGameData().valid )
  {
    // HACK: needed by SimSpark - overide the player number
    if(getGameData().newPlayerNumber > 0) {
      getPlayerInfo().playerNumber = getGameData().newPlayerNumber;
    }

    getPlayerInfo().update(getGameData());

    // reset return message if old message was accepted
    if( returnMessage == GameReturnData::manual_penalise
        && getGameData().getOwnRobotInfo(getPlayerInfo().playerNumber).penalty != GameData::none)
    {
      returnMessage = GameReturnData::alive;
    }
    else if(returnMessage == GameReturnData::manual_unpenalise
            && getGameData().getOwnRobotInfo(getPlayerInfo().playerNumber).penalty == GameData::none)
    {
      returnMessage = GameReturnData::alive;
    }
  }

  // keep the manual penalized state
  if(returnMessage == GameReturnData::manual_penalise) {
    getPlayerInfo().robotState = PlayerInfo::penalized;
  }

  handleButtons();
  handleHeadButtons();
  handleDebugRequest();

  // remember the whistle counter before set
  if(getPlayerInfo().robotState == PlayerInfo::ready) {
    lastWhistleCount = getWhistlePercept().counter;
  }
  // whistle overrides gamecontroller when in set
  else if(getGameData().gameState == GameData::set)
  {
    // switch from set to play
    if(getWhistlePercept().counter > lastWhistleCount) {
      getPlayerInfo().robotState = PlayerInfo::playing;
    }
  }

  if(  oldRobotState != getPlayerInfo().robotState
    || oldTeamColor  != getPlayerInfo().teamColor
    || getPlayerInfo().robotState == PlayerInfo::initial)
  {
    updateLEDs();
  }

  // provide the return message
  getGameReturnData().team = getPlayerInfo().teamNumber;
  getGameReturnData().player = getPlayerInfo().playerNumber;
  getGameReturnData().message = returnMessage;
} // end execute

void GameController::handleDebugRequest()
{
  PlayerInfo::RobotState debugState = getPlayerInfo().robotState;

  DEBUG_REQUEST("gamecontroller:initial",
    debugState = PlayerInfo::initial;
  );
  DEBUG_REQUEST("gamecontroller:ready",
    debugState = PlayerInfo::ready;
  );
  DEBUG_REQUEST("gamecontroller:set",
    debugState = PlayerInfo::set;
  );
  DEBUG_REQUEST("gamecontroller:play",
    debugState = PlayerInfo::playing;
  );
  DEBUG_REQUEST("gamecontroller:penalized",
    debugState = PlayerInfo::penalized;
  );

  // NOTE: same behavior as the button interface
  if(debugState != getPlayerInfo().robotState) {
    getPlayerInfo().robotState = debugState;

    // NOTE: logic is reverted in relation to button interface
    switch (getPlayerInfo().robotState)
    {
    case PlayerInfo::initial:
    case PlayerInfo::ready:
    case PlayerInfo::set:
    case PlayerInfo::playing:
    case PlayerInfo::finished:
    {
      returnMessage = GameReturnData::manual_unpenalise;
      break;
    }
    case PlayerInfo::penalized:
    {
      returnMessage = GameReturnData::manual_penalise;
      break;
    }
    default:
      ASSERT(false);
    }
  }

} // end handleDebugRequest

void GameController::handleButtons()
{
  if (getButtonState()[ButtonState::Chest] == ButtonEvent::CLICKED)
  {
    switch (getPlayerInfo().robotState)
    {
    case PlayerInfo::initial:
    case PlayerInfo::ready:
    case PlayerInfo::set:
    case PlayerInfo::playing:
    case PlayerInfo::finished:
    {
      getPlayerInfo().robotState = PlayerInfo::penalized;
      returnMessage = GameReturnData::manual_penalise;
      break;
    }
    case PlayerInfo::penalized:
    {
      getPlayerInfo().robotState = PlayerInfo::playing;
      returnMessage = GameReturnData::manual_unpenalise;
      break;
    }
    default:
      ASSERT(false);
    }
  }

  // re-set team color or kickoff in initial
  if (getPlayerInfo().robotState == PlayerInfo::initial)
  {
    if ( getButtonState()[ButtonState::LeftFoot] == ButtonEvent::PRESSED )
    {
      // switch team color
      GameData::TeamColor oldColor = getPlayerInfo().teamColor;
      if (oldColor == GameData::blue) {
        getPlayerInfo().teamColor = GameData::red;
      } else if (oldColor == GameData::red) {
        getPlayerInfo().teamColor = GameData::yellow;
      } else if (oldColor == GameData::yellow) {
        getPlayerInfo().teamColor = GameData::black;
      } else if (oldColor == GameData::black) {
        getPlayerInfo().teamColor = GameData::blue;
      } else { // set to red by default
        getPlayerInfo().teamColor = GameData::red;
      }
    }

    if ( getButtonState()[ButtonState::RightFoot] == ButtonEvent::PRESSED )
    {
      // switch kick off team
      getPlayerInfo().kickoff = !getPlayerInfo().kickoff;
    }
  }

  // go back from penalized to initial both foot bumpers are pressed for more than 1s
  else if (getPlayerInfo().robotState == PlayerInfo::penalized &&
    (  getButtonState()[ButtonState::LeftFoot].isPressed &&
       getFrameInfo().getTimeSince(getButtonState()[ButtonState::LeftFoot].timeOfLastEvent) > 1000 )
    &&
    (  getButtonState()[ButtonState::RightFoot].isPressed &&
       getFrameInfo().getTimeSince(getButtonState()[ButtonState::RightFoot].timeOfLastEvent) > 1000 )
    )
  {
    getPlayerInfo().robotState = PlayerInfo::initial;
    returnMessage = GameReturnData::manual_unpenalise;
  }

} // end handleButtons

void GameController::handleHeadButtons()
{
  getSoundPlayData().mute = true;
  getSoundPlayData().soundFile = "";

  if(   getButtonState().buttons[ButtonState::HeadMiddle] == ButtonEvent::CLICKED
     && getPlayerInfo().robotState == PlayerInfo::initial)
  {
    int playerNumber = getPlayerInfo().playerNumber;
    if(playerNumber <= 9)
    {
      std::stringstream ssWav;
      ssWav << playerNumber;
      ssWav << ".wav";
      getSoundPlayData().mute = false;
      getSoundPlayData().soundFile = ssWav.str();
    }
  }
}

void GameController::updateLEDs()
{
  // reset
  getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::RED] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::GREEN] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::BLUE] = 0.0;

  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::RED] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::GREEN] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::BLUE] = 0.0;

  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::RED] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::GREEN] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::BLUE] = 0.0;

  for(unsigned int i=LEDData::HeadFrontLeft0; i <= LEDData::HeadRearRight2; i++)
  {
    getGameControllerLEDRequest().request.theMonoLED[i] = 0.0;
  }

  // show game state in torso
  switch (getPlayerInfo().robotState)
  {
    case PlayerInfo::ready:
        getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::BLUE] = 1.0;
      break;
    case PlayerInfo::set:
        getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::GREEN] = 1.0;
      getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::RED] = 1.0;
      break;
    case PlayerInfo::playing:
        getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::GREEN] = 1.0;
      break;
    case PlayerInfo::penalized:
        getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::RED] = 1.0;
      break;
    default:
      break;
  }

  // show team color on left foot
  if (getPlayerInfo().teamColor == GameData::red)
  {
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::RED] = 0.3;
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::BLUE] = 0.1;
  }
  else if (getPlayerInfo().teamColor == GameData::blue)
  {
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::BLUE] = 1.0;
  }
  else if(getPlayerInfo().teamColor == GameData::yellow)
  {
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::RED] = 1.0;
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::GREEN] = 1.0;
  }
  else if(getPlayerInfo().teamColor == GameData::black)
  {
      // LED off
  }

  // show kickoff state on right foot and head in initial, ready and set
  if (getPlayerInfo().robotState == PlayerInfo::initial
    || getPlayerInfo().robotState == PlayerInfo::ready
    || getPlayerInfo().robotState == PlayerInfo::set)
  {
    if (getPlayerInfo().kickoff)
    {
      getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::RED] = 0.7;
      getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::GREEN] = 1.0;
      getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::BLUE] = 1.0;

      for(unsigned int i=LEDData::HeadFrontLeft0; i <= LEDData::HeadRearRight2; i++)
      {
        getGameControllerLEDRequest().request.theMonoLED[i] = 1.0;
      }
    }
  }
} // end updateLEDs

GameController::~GameController()
{
}
<commit_msg>trigger finished game state by debug request<commit_after>#include "GameController.h"

#include "Tools/Debug/DebugRequest.h"
#include <PlatformInterface/Platform.h>

GameController::GameController()
  :
  lastWhistleCount(0),
  returnMessage(GameReturnData::alive)
{
  DEBUG_REQUEST_REGISTER("gamecontroller:play", "force the play state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:penalized", "force the penalized state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:initial", "force the initial state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:ready", "force the ready state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:set", "force the set state", false);
  DEBUG_REQUEST_REGISTER("gamecontroller:finished", "force the finished state", false);

  // TODO: make it parameters?
  // load values from config
  const Configuration& config = naoth::Platform::getInstance().theConfiguration;

  if (config.hasKey("player", "NumOfPlayer")) {
    getPlayerInfo().playersPerTeam = config.getInt("player", "NumOfPlayer");
  } else {
    std::cerr << "[GameData] " << "No number of players (NumOfPlayers) given" << std::endl;
  }

  if (config.hasKey("player", "PlayerNumber")) {
    getPlayerInfo().playerNumber = config.getInt("player", "PlayerNumber");
  } else {
    std::cerr << "[PlayerInfo] " << "No player number (PlayerNumber) given" << std::endl;
    getPlayerInfo().playerNumber = 3;
  }

  if (config.hasKey("player", "TeamNumber")) {
    getPlayerInfo().teamNumber = config.getInt("player", "TeamNumber");
  } else {
    std::cerr << "[PlayerInfo] " << "No team number (TeamNumber) given" << std::endl;
    getPlayerInfo().teamNumber = 0;
  }

  if (config.hasKey("player", "TeamName")) {
    getPlayerInfo().teamName = config.getString("player", "TeamName");
  } else {
    std::cerr << "[PlayerInfo] " << "No team name (TeamName) given" << std::endl;
    getPlayerInfo().teamName = "unknown";
  }

  // NOTE: default team color is red
  getPlayerInfo().teamColor = GameData::red;
  if (config.hasKey("player", "TeamColor"))
  {
    getPlayerInfo().teamColor = GameData::teamColorFromString(config.getString("player", "TeamColor"));
    if (getPlayerInfo().teamColor == GameData::unknown_team_color)
    {
      std::cerr << "[GameData] " << "Invalid team color (TeamColor) \""
        << config.getString("player", "TeamColor") << "\" given" << std::endl;
    }
  } else {
    std::cerr << "[GameData] " << "No team color (TeamColor) given" << std::endl;
  }
}

void GameController::execute()
{
  PlayerInfo::RobotState oldRobotState = getPlayerInfo().robotState;
  GameData::TeamColor oldTeamColor = getPlayerInfo().teamColor;

  // try update from the game controller message
  if ( getGameData().valid )
  {
    // HACK: needed by SimSpark - overide the player number
    if(getGameData().newPlayerNumber > 0) {
      getPlayerInfo().playerNumber = getGameData().newPlayerNumber;
    }

    getPlayerInfo().update(getGameData());

    // reset return message if old message was accepted
    if( returnMessage == GameReturnData::manual_penalise
        && getGameData().getOwnRobotInfo(getPlayerInfo().playerNumber).penalty != GameData::none)
    {
      returnMessage = GameReturnData::alive;
    }
    else if(returnMessage == GameReturnData::manual_unpenalise
            && getGameData().getOwnRobotInfo(getPlayerInfo().playerNumber).penalty == GameData::none)
    {
      returnMessage = GameReturnData::alive;
    }
  }

  // keep the manual penalized state
  if(returnMessage == GameReturnData::manual_penalise) {
    getPlayerInfo().robotState = PlayerInfo::penalized;
  }

  handleButtons();
  handleHeadButtons();
  handleDebugRequest();

  // remember the whistle counter before set
  if(getPlayerInfo().robotState == PlayerInfo::ready) {
    lastWhistleCount = getWhistlePercept().counter;
  }
  // whistle overrides gamecontroller when in set
  else if(getGameData().gameState == GameData::set)
  {
    // switch from set to play
    if(getWhistlePercept().counter > lastWhistleCount) {
      getPlayerInfo().robotState = PlayerInfo::playing;
    }
  }

  if(  oldRobotState != getPlayerInfo().robotState
    || oldTeamColor  != getPlayerInfo().teamColor
    || getPlayerInfo().robotState == PlayerInfo::initial)
  {
    updateLEDs();
  }

  // provide the return message
  getGameReturnData().team = getPlayerInfo().teamNumber;
  getGameReturnData().player = getPlayerInfo().playerNumber;
  getGameReturnData().message = returnMessage;
} // end execute

void GameController::handleDebugRequest()
{
  PlayerInfo::RobotState debugState = getPlayerInfo().robotState;

  DEBUG_REQUEST("gamecontroller:initial",
    debugState = PlayerInfo::initial;
  );
  DEBUG_REQUEST("gamecontroller:ready",
    debugState = PlayerInfo::ready;
  );
  DEBUG_REQUEST("gamecontroller:set",
    debugState = PlayerInfo::set;
  );
  DEBUG_REQUEST("gamecontroller:play",
    debugState = PlayerInfo::playing;
  );
  DEBUG_REQUEST("gamecontroller:penalized",
    debugState = PlayerInfo::penalized;
  );
  DEBUG_REQUEST("gamecontroller:finished",
    debugState = PlayerInfo::finished;
  );

  // NOTE: same behavior as the button interface
  if(debugState != getPlayerInfo().robotState) {
    getPlayerInfo().robotState = debugState;

    // NOTE: logic is reverted in relation to button interface
    switch (getPlayerInfo().robotState)
    {
    case PlayerInfo::initial:
    case PlayerInfo::ready:
    case PlayerInfo::set:
    case PlayerInfo::playing:
    case PlayerInfo::finished:
    {
      returnMessage = GameReturnData::manual_unpenalise;
      break;
    }
    case PlayerInfo::penalized:
    {
      returnMessage = GameReturnData::manual_penalise;
      break;
    }
    default:
      ASSERT(false);
    }
  }

} // end handleDebugRequest

void GameController::handleButtons()
{
  if (getButtonState()[ButtonState::Chest] == ButtonEvent::CLICKED)
  {
    switch (getPlayerInfo().robotState)
    {
    case PlayerInfo::initial:
    case PlayerInfo::ready:
    case PlayerInfo::set:
    case PlayerInfo::playing:
    case PlayerInfo::finished:
    {
      getPlayerInfo().robotState = PlayerInfo::penalized;
      returnMessage = GameReturnData::manual_penalise;
      break;
    }
    case PlayerInfo::penalized:
    {
      getPlayerInfo().robotState = PlayerInfo::playing;
      returnMessage = GameReturnData::manual_unpenalise;
      break;
    }
    default:
      ASSERT(false);
    }
  }

  // re-set team color or kickoff in initial
  if (getPlayerInfo().robotState == PlayerInfo::initial)
  {
    if ( getButtonState()[ButtonState::LeftFoot] == ButtonEvent::PRESSED )
    {
      // switch team color
      GameData::TeamColor oldColor = getPlayerInfo().teamColor;
      if (oldColor == GameData::blue) {
        getPlayerInfo().teamColor = GameData::red;
      } else if (oldColor == GameData::red) {
        getPlayerInfo().teamColor = GameData::yellow;
      } else if (oldColor == GameData::yellow) {
        getPlayerInfo().teamColor = GameData::black;
      } else if (oldColor == GameData::black) {
        getPlayerInfo().teamColor = GameData::blue;
      } else { // set to red by default
        getPlayerInfo().teamColor = GameData::red;
      }
    }

    if ( getButtonState()[ButtonState::RightFoot] == ButtonEvent::PRESSED )
    {
      // switch kick off team
      getPlayerInfo().kickoff = !getPlayerInfo().kickoff;
    }
  }

  // go back from penalized to initial both foot bumpers are pressed for more than 1s
  else if (getPlayerInfo().robotState == PlayerInfo::penalized &&
    (  getButtonState()[ButtonState::LeftFoot].isPressed &&
       getFrameInfo().getTimeSince(getButtonState()[ButtonState::LeftFoot].timeOfLastEvent) > 1000 )
    &&
    (  getButtonState()[ButtonState::RightFoot].isPressed &&
       getFrameInfo().getTimeSince(getButtonState()[ButtonState::RightFoot].timeOfLastEvent) > 1000 )
    )
  {
    getPlayerInfo().robotState = PlayerInfo::initial;
    returnMessage = GameReturnData::manual_unpenalise;
  }

} // end handleButtons

void GameController::handleHeadButtons()
{
  getSoundPlayData().mute = true;
  getSoundPlayData().soundFile = "";

  if(   getButtonState().buttons[ButtonState::HeadMiddle] == ButtonEvent::CLICKED
     && getPlayerInfo().robotState == PlayerInfo::initial)
  {
    int playerNumber = getPlayerInfo().playerNumber;
    if(playerNumber <= 9)
    {
      std::stringstream ssWav;
      ssWav << playerNumber;
      ssWav << ".wav";
      getSoundPlayData().mute = false;
      getSoundPlayData().soundFile = ssWav.str();
    }
  }
}

void GameController::updateLEDs()
{
  // reset
  getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::RED] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::GREEN] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::BLUE] = 0.0;

  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::RED] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::GREEN] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::BLUE] = 0.0;

  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::RED] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::GREEN] = 0.0;
  getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::BLUE] = 0.0;

  for(unsigned int i=LEDData::HeadFrontLeft0; i <= LEDData::HeadRearRight2; i++)
  {
    getGameControllerLEDRequest().request.theMonoLED[i] = 0.0;
  }

  // show game state in torso
  switch (getPlayerInfo().robotState)
  {
    case PlayerInfo::ready:
        getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::BLUE] = 1.0;
      break;
    case PlayerInfo::set:
        getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::GREEN] = 1.0;
      getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::RED] = 1.0;
      break;
    case PlayerInfo::playing:
        getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::GREEN] = 1.0;
      break;
    case PlayerInfo::penalized:
        getGameControllerLEDRequest().request.theMultiLED[LEDData::ChestButton][LEDData::RED] = 1.0;
      break;
    default:
      break;
  }

  // show team color on left foot
  if (getPlayerInfo().teamColor == GameData::red)
  {
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::RED] = 0.3;
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::BLUE] = 0.1;
  }
  else if (getPlayerInfo().teamColor == GameData::blue)
  {
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::BLUE] = 1.0;
  }
  else if(getPlayerInfo().teamColor == GameData::yellow)
  {
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::RED] = 1.0;
    getGameControllerLEDRequest().request.theMultiLED[LEDData::FootLeft][LEDData::GREEN] = 1.0;
  }
  else if(getPlayerInfo().teamColor == GameData::black)
  {
      // LED off
  }

  // show kickoff state on right foot and head in initial, ready and set
  if (getPlayerInfo().robotState == PlayerInfo::initial
    || getPlayerInfo().robotState == PlayerInfo::ready
    || getPlayerInfo().robotState == PlayerInfo::set)
  {
    if (getPlayerInfo().kickoff)
    {
      getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::RED] = 0.7;
      getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::GREEN] = 1.0;
      getGameControllerLEDRequest().request.theMultiLED[LEDData::FootRight][LEDData::BLUE] = 1.0;

      for(unsigned int i=LEDData::HeadFrontLeft0; i <= LEDData::HeadRearRight2; i++)
      {
        getGameControllerLEDRequest().request.theMonoLED[i] = 1.0;
      }
    }
  }
} // end updateLEDs

GameController::~GameController()
{
}
<|endoftext|>


<commit_before>//
//  MathUtils.cpp
//  Bomber
//
//  Created by Jeremias Nunez on 3/20/14.
//
//

#include "MathUtils.h"
#include <random>

int MathUtils::randIntInRange(int min, int max)
{
    return min + (rand() % (int)(max - min + 1));
}<commit_msg>changed random int generation algorithm to use better rand function<commit_after>//
//  MathUtils.cpp
//  Bomber
//
//  Created by Jeremias Nunez on 3/20/14.
//
//

#include "MathUtils.h"
#include <random>

int MathUtils::randIntInRange(int min, int max)
{
    return min + (arc4random() % (int)(max - min + 1));
}<|endoftext|>