<commit_before>// ======================================================================== //
// Copyright 2009-2019 Intel Corporation                                    //
//                                                                          //
// Licensed under the Apache License, Version 2.0 (the "License");          //
// you may not use this file except in compliance with the License.         //
// You may obtain a copy of the License at                                  //
//                                                                          //
//     http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                          //
// Unless required by applicable law or agreed to in writing, software      //
// distributed under the License is distributed on an "AS IS" BASIS,        //
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
// See the License for the specific language governing permissions and      //
// limitations under the License.                                           //
// ======================================================================== //

#include "ospray/ospray_util.h"

extern "C" {

// OSPData helpers ////////////////////////////////////////////////////////////

OSPData ospNewSharedData1D(const void *sharedData,
                           OSPDataType type,
                           uint32_t numItems)
{
  return ospNewSharedData(sharedData, type, numItems, 0, 1, 0, 1, 0);
}

OSPData ospNewSharedData1DStride(const void *sharedData,
                                 OSPDataType type,
                                 uint32_t numItems,
                                 int64_t byteStride)
{
  return ospNewSharedData(sharedData, type, numItems, byteStride, 1, 0, 1, 0);
}

OSPData ospNewSharedData2D(const void *sharedData,
                           OSPDataType type,
                           uint32_t numItems1,
                           uint32_t numItems2)
{
  return ospNewSharedData(sharedData, type, numItems1, 0, numItems2, 0, 1, 0);
}

OSPData ospNewSharedData2DStride(const void *sharedData,
                                 OSPDataType type,
                                 uint32_t numItems1,
                                 int64_t byteStride1,
                                 uint32_t numItems2,
                                 int64_t byteStride2)
{
  return ospNewSharedData(
      sharedData, type, numItems1, byteStride1, numItems2, byteStride2, 1, 0);
}

OSPData ospNewSharedData3D(const void *sharedData,
                           OSPDataType type,
                           uint32_t numItems1,
                           uint32_t numItems2,
                           uint32_t numItems3)
{
  return ospNewSharedData(
      sharedData, type, numItems1, 0, numItems2, 0, numItems3, 0);
}

OSPData ospNewData1D(OSPDataType type, uint32_t numItems)
{
  return ospNewData(type, numItems, 1, 1);
}

OSPData ospNewData2D(OSPDataType type, uint32_t numItems1, uint32_t numItems2)
{
  return ospNewData(type, numItems1, numItems2, 1);
}

void ospCopyData1D(const OSPData source,
                   OSPData destination,
                   uint32_t destinationIndex)
{
  ospCopyData(source, destination, destinationIndex, 0, 0);
}

void ospCopyData2D(const OSPData source,
                   OSPData destination,
                   uint32_t destinationIndex1,
                   uint32_t destinationIndex2)
{
  ospCopyData(source, destination, destinationIndex1, destinationIndex2, 0);
}

// Parameter helpers ////////////////////////////////////////////////////////

void ospSetString(OSPObject o, const char *id, const char *s)
{
  ospSetParam(o, id, OSP_STRING, &s);
}

void ospSetObject(OSPObject o, const char *id, OSPObject other)
{
  ospSetParam(o, id, OSP_OBJECT, &other);
}

void ospSetBool(OSPObject o, const char *id, int x)
{
  ospSetParam(o, id, OSP_BOOL, &x);
}

void ospSetFloat(OSPObject o, const char *id, float x)
{
  ospSetParam(o, id, OSP_FLOAT, &x);
}

void ospSetInt(OSPObject o, const char *id, int x)
{
  ospSetParam(o, id, OSP_INT, &x);
}

void ospSetVec2f(OSPObject o, const char *id, float x, float y)
{
  float v[] = {x, y};
  ospSetParam(o, id, OSP_VEC2F, v);
}

void ospSetVec3f(OSPObject o, const char *id, float x, float y, float z)
{
  float v[] = {x, y, z};
  ospSetParam(o, id, OSP_VEC3F, v);
}

void ospSetVec4f(
    OSPObject o, const char *id, float x, float y, float z, float w)
{
  float v[] = {x, y, z, w};
  ospSetParam(o, id, OSP_VEC4F, v);
}

void ospSetVec2i(OSPObject o, const char *id, int x, int y)
{
  int v[] = {x, y};
  ospSetParam(o, id, OSP_VEC2I, v);
}

void ospSetVec3i(OSPObject o, const char *id, int x, int y, int z)
{
  int v[] = {x, y, z};
  ospSetParam(o, id, OSP_VEC3I, v);
}

void ospSetVec4i(OSPObject o, const char *id, int x, int y, int z, int w)
{
  int v[] = {x, y, z, w};
  ospSetParam(o, id, OSP_VEC4I, v);
}

void ospSetObjectAsData(OSPObject o,
                        const char *n,
                        OSPDataType type,
                        OSPObject p)
{
  OSPData data = ospNewData(1, type, &p);
  ospSetObject(o, n, data);
  ospRelease(data);
}

// Rendering helpers //////////////////////////////////////////////////////////

float ospRenderFrameBlocking(OSPFrameBuffer fb,
                             OSPRenderer renderer,
                             OSPCamera camera,
                             OSPWorld world)
{
  OSPFuture f = ospRenderFrame(fb, renderer, camera, world);
  ospRelease(f);
  return ospGetVariance(fb);
}

}  // extern "C"

// XXX temporary, to maintain backwards compatibility
OSPData ospNewData(size_t numItems,
                   OSPDataType type,
                   const void *source,
                   uint32_t dataCreationFlags)
{
  if (dataCreationFlags)
    return ospNewSharedData(source, type, numItems);
  else {
    OSPData src = ospNewSharedData(source, type, numItems);
    OSPData dst = ospNewData(type, numItems);
    ospCopyData(src, dst);
    ospRelease(src);
    return dst;
  }
}
<commit_msg>fix missing ospWait() in ospRenderFrameBlocking()<commit_after>// ======================================================================== //
// Copyright 2009-2019 Intel Corporation                                    //
//                                                                          //
// Licensed under the Apache License, Version 2.0 (the "License");          //
// you may not use this file except in compliance with the License.         //
// You may obtain a copy of the License at                                  //
//                                                                          //
//     http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                          //
// Unless required by applicable law or agreed to in writing, software      //
// distributed under the License is distributed on an "AS IS" BASIS,        //
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
// See the License for the specific language governing permissions and      //
// limitations under the License.                                           //
// ======================================================================== //

#include "ospray/ospray_util.h"

extern "C" {

// OSPData helpers ////////////////////////////////////////////////////////////

OSPData ospNewSharedData1D(const void *sharedData,
                           OSPDataType type,
                           uint32_t numItems)
{
  return ospNewSharedData(sharedData, type, numItems, 0, 1, 0, 1, 0);
}

OSPData ospNewSharedData1DStride(const void *sharedData,
                                 OSPDataType type,
                                 uint32_t numItems,
                                 int64_t byteStride)
{
  return ospNewSharedData(sharedData, type, numItems, byteStride, 1, 0, 1, 0);
}

OSPData ospNewSharedData2D(const void *sharedData,
                           OSPDataType type,
                           uint32_t numItems1,
                           uint32_t numItems2)
{
  return ospNewSharedData(sharedData, type, numItems1, 0, numItems2, 0, 1, 0);
}

OSPData ospNewSharedData2DStride(const void *sharedData,
                                 OSPDataType type,
                                 uint32_t numItems1,
                                 int64_t byteStride1,
                                 uint32_t numItems2,
                                 int64_t byteStride2)
{
  return ospNewSharedData(
      sharedData, type, numItems1, byteStride1, numItems2, byteStride2, 1, 0);
}

OSPData ospNewSharedData3D(const void *sharedData,
                           OSPDataType type,
                           uint32_t numItems1,
                           uint32_t numItems2,
                           uint32_t numItems3)
{
  return ospNewSharedData(
      sharedData, type, numItems1, 0, numItems2, 0, numItems3, 0);
}

OSPData ospNewData1D(OSPDataType type, uint32_t numItems)
{
  return ospNewData(type, numItems, 1, 1);
}

OSPData ospNewData2D(OSPDataType type, uint32_t numItems1, uint32_t numItems2)
{
  return ospNewData(type, numItems1, numItems2, 1);
}

void ospCopyData1D(const OSPData source,
                   OSPData destination,
                   uint32_t destinationIndex)
{
  ospCopyData(source, destination, destinationIndex, 0, 0);
}

void ospCopyData2D(const OSPData source,
                   OSPData destination,
                   uint32_t destinationIndex1,
                   uint32_t destinationIndex2)
{
  ospCopyData(source, destination, destinationIndex1, destinationIndex2, 0);
}

// Parameter helpers ////////////////////////////////////////////////////////

void ospSetString(OSPObject o, const char *id, const char *s)
{
  ospSetParam(o, id, OSP_STRING, &s);
}

void ospSetObject(OSPObject o, const char *id, OSPObject other)
{
  ospSetParam(o, id, OSP_OBJECT, &other);
}

void ospSetBool(OSPObject o, const char *id, int x)
{
  ospSetParam(o, id, OSP_BOOL, &x);
}

void ospSetFloat(OSPObject o, const char *id, float x)
{
  ospSetParam(o, id, OSP_FLOAT, &x);
}

void ospSetInt(OSPObject o, const char *id, int x)
{
  ospSetParam(o, id, OSP_INT, &x);
}

void ospSetVec2f(OSPObject o, const char *id, float x, float y)
{
  float v[] = {x, y};
  ospSetParam(o, id, OSP_VEC2F, v);
}

void ospSetVec3f(OSPObject o, const char *id, float x, float y, float z)
{
  float v[] = {x, y, z};
  ospSetParam(o, id, OSP_VEC3F, v);
}

void ospSetVec4f(
    OSPObject o, const char *id, float x, float y, float z, float w)
{
  float v[] = {x, y, z, w};
  ospSetParam(o, id, OSP_VEC4F, v);
}

void ospSetVec2i(OSPObject o, const char *id, int x, int y)
{
  int v[] = {x, y};
  ospSetParam(o, id, OSP_VEC2I, v);
}

void ospSetVec3i(OSPObject o, const char *id, int x, int y, int z)
{
  int v[] = {x, y, z};
  ospSetParam(o, id, OSP_VEC3I, v);
}

void ospSetVec4i(OSPObject o, const char *id, int x, int y, int z, int w)
{
  int v[] = {x, y, z, w};
  ospSetParam(o, id, OSP_VEC4I, v);
}

void ospSetObjectAsData(OSPObject o,
                        const char *n,
                        OSPDataType type,
                        OSPObject p)
{
  OSPData data = ospNewData(1, type, &p);
  ospSetObject(o, n, data);
  ospRelease(data);
}

// Rendering helpers //////////////////////////////////////////////////////////

float ospRenderFrameBlocking(OSPFrameBuffer fb,
                             OSPRenderer renderer,
                             OSPCamera camera,
                             OSPWorld world)
{
  OSPFuture f = ospRenderFrame(fb, renderer, camera, world);
  ospWait(f, OSP_TASK_FINISHED);
  ospRelease(f);
  return ospGetVariance(fb);
}

}  // extern "C"

// XXX temporary, to maintain backwards compatibility
OSPData ospNewData(size_t numItems,
                   OSPDataType type,
                   const void *source,
                   uint32_t dataCreationFlags)
{
  if (dataCreationFlags)
    return ospNewSharedData(source, type, numItems);
  else {
    OSPData src = ospNewSharedData(source, type, numItems);
    OSPData dst = ospNewData(type, numItems);
    ospCopyData(src, dst);
    ospRelease(src);
    return dst;
  }
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2012, Willow Garage, Inc.
 * 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 <ORGANIZATION> 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: Jon Binney

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <tf/transform_listener.h>
#include <moveit/plan_execution/plan_execution.h>
#include <moveit/kinematics_planner/kinematics_planner.h>
#include <moveit/trajectory_processing/trajectory_tools.h>

#include <moveit_msgs/PositionConstraint.h>
#include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>

static const std::string ROBOT_DESCRIPTION = "robot_description";    // name of the robot description (a param name, so it can be changed externally)
static const std::string NODE_NAME = "test_kinematics_planner";                   // name of node
static const std::string GROUP_NAME = "left_arm";
static const std::string END_EFFECTOR_FRAME_NAME = "l_wrist_roll_link";
static const std::string END_EFFECTOR_LINK_NAME = "l_wrist_roll_link";
static const ros::Duration ALLOWED_PLANNING_TIME(5.0);
static const int NUM_PLANNING_ATTEMPTS = 1;
static const ros::Duration WAIT_LOOP_DURATION(0.01);
static const double INTERPOLATED_IK_PLANNING_TIME = 5.0;

void make_grasp_marker(const std::string &ns, const geometry_msgs::PoseStamped &ps, visualization_msgs::Marker &m)
{
    m.action = visualization_msgs::Marker::ADD;
    m.header = ps.header;
    m.ns = ns;
    m.id = 0;
    m.type = visualization_msgs::Marker::ARROW;
    m.pose = ps.pose;
    m.color.r = 1.0;
    m.color.a = 1.0;
    m.scale.x = 0.2;
    m.scale.y = 0.02;
    m.scale.z = 0.02;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, NODE_NAME);
    ros::NodeHandle nh;

    ros::AsyncSpinner spinner(4); // Use 4 threads
    spinner.start();

    ROS_INFO("Advertising markers");
    ros::Duration(1.0).sleep();
    ros::Publisher pub = nh.advertise<visualization_msgs::Marker>("grasp_debug", 5);

    ROS_INFO("Waiting one second for subscribers");
    ros::Duration(1.0).sleep();

    ROS_INFO("Creating planning Scene");
    boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener());
    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION, tf));
    if (planning_scene_monitor->getPlanningScene() && planning_scene_monitor->getPlanningScene()->isConfigured())
    {
        planning_scene_monitor->startWorldGeometryMonitor();
        planning_scene_monitor->startSceneMonitor();
        planning_scene_monitor->startStateMonitor();
    }
    else
    {
        ROS_ERROR("Planning scene not configured");
        return -1;
    }

    plan_execution::PlanExecution plan_execution(planning_scene_monitor);

    ROS_INFO("Waiting for robot state");
    std::vector<std::string> missing_joints;
    while(!planning_scene_monitor->getStateMonitor()->haveCompleteState(missing_joints))
    {
        ROS_INFO("Waiting for current joint states");
        for(unsigned int joint_i = 0; joint_i < missing_joints.size(); joint_i++)
            ROS_INFO("Missing joint: %s", missing_joints[joint_i].c_str());
        WAIT_LOOP_DURATION.sleep();
    }

    /* get the current gripper pose */
    planning_scene_monitor->updateFrameTransforms();
    kinematic_state::KinematicState kinematic_state = planning_scene_monitor->getPlanningScene()->getCurrentState();
    const Eigen::Affine3d *gripper_pose = kinematic_state.getFrameTransform(END_EFFECTOR_LINK_NAME);
    ROS_INFO_STREAM("Current gripper pose:" << (gripper_pose->matrix()));

    Eigen::Vector3d t = gripper_pose->translation();
    Eigen::Quaterniond q;
    q = gripper_pose->rotation();

    ROS_INFO("Current gripper trans: %f %f %f", t.x(), t.y(), t.z());
    ROS_INFO("Current gripper rot: %f %f %f %f", q.x(), q.y(), q.z(), q.w());

    /* start position is current position */
    geometry_msgs::PoseStamped ps_start, ps_end;
    ps_start.header.frame_id = "base_link";
    ps_start.header.stamp = ros::Time::now();
    ps_start.pose.position.x = t.x();
    ps_start.pose.position.y = t.y();
    ps_start.pose.position.z = t.z();
    ps_start.pose.orientation.x = q.x();
    ps_start.pose.orientation.y = q.y();
    ps_start.pose.orientation.z = q.z();
    ps_start.pose.orientation.w = q.w();

    /* end position is start position shifted a bit */
    ps_end = ps_start;
    ps_end.pose.position.x -= 0.1;

    /* publish rviz markers for the start and end poses */
    visualization_msgs::Marker m_start, m_end;
    make_grasp_marker("iik_start_pose", ps_start, m_start);
    make_grasp_marker("iik_end_pose", ps_end, m_end);
    pub.publish(m_start);
    pub.publish(m_end);

    /* create an interpolated IK planner */
    kinematics_planner::KinematicsPlanner kin_planner;
    std::vector<std::string> group_names;
    group_names.push_back(GROUP_NAME);
    kin_planner.initialize(group_names, planning_scene_monitor->getKinematicModel());

    std::map<std::string,geometry_msgs::PoseStamped> start_request;
    start_request[GROUP_NAME] = ps_start;
    std::map<std::string,geometry_msgs::PoseStamped> goal_request;
    goal_request[GROUP_NAME] = ps_end;
    moveit_msgs::Constraints path_constraints;
    moveit_msgs::RobotTrajectory robot_trajectory;
    moveit_msgs::MoveItErrorCodes error_code;

    /* create plan using interpolated IK planner */
    bool r = kin_planner.solve(start_request,
                               goal_request,
                               planning_scene_monitor->getPlanningScene(),
                               path_constraints,
                               INTERPOLATED_IK_PLANNING_TIME,
                               robot_trajectory,
                               error_code);
    if(r)
    {
        ROS_INFO("Got solution from kinematics planner");
        ROS_INFO_STREAM("Solution: " << robot_trajectory);
    }
    else
    {
        ROS_INFO("Unable to solve. Error code: %d", error_code.val);
    }

    /* execute the planned trajectory */
    ROS_INFO("Executing trajectory");
    plan_execution.getTrajectoryExecutionManager().clear();
    if(plan_execution.getTrajectoryExecutionManager().push(robot_trajectory))
    {
      plan_execution.getTrajectoryExecutionManager().execute();

      /* wait for the trajectory to complete */
      moveit_controller_manager::ExecutionStatus es = plan_execution.getTrajectoryExecutionManager().waitForExecution();
      if (es == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
        ROS_INFO("Trajectory execution succeeded");
      else
        if (es == moveit_controller_manager::ExecutionStatus::PREEMPTED)
            ROS_INFO("Trajectory execution preempted");
        else
          if (es == moveit_controller_manager::ExecutionStatus::TIMED_OUT)
              ROS_INFO("Trajectory execution timed out");
          else
              ROS_INFO("Trajectory execution control failed");
    }
    else
    {
      ROS_INFO("Failed to push trajectory");
      return -1;
    }

    planning_scene_monitor->stopSceneMonitor();

    ros::spin();
    return 0;
}
<commit_msg>forgot to fix this<commit_after>/*
 * Copyright (c) 2012, Willow Garage, Inc.
 * 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 <ORGANIZATION> 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: Jon Binney

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <tf/transform_listener.h>
#include <moveit/plan_execution/plan_execution.h>
#include <moveit/kinematics_planner/kinematics_planner.h>
#include <moveit/trajectory_processing/trajectory_tools.h>

#include <moveit_msgs/PositionConstraint.h>
#include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>

static const std::string ROBOT_DESCRIPTION = "robot_description";    // name of the robot description (a param name, so it can be changed externally)
static const std::string NODE_NAME = "test_kinematics_planner";                   // name of node
static const std::string GROUP_NAME = "left_arm";
static const std::string END_EFFECTOR_FRAME_NAME = "l_wrist_roll_link";
static const std::string END_EFFECTOR_LINK_NAME = "l_wrist_roll_link";
static const ros::Duration ALLOWED_PLANNING_TIME(5.0);
static const int NUM_PLANNING_ATTEMPTS = 1;
static const ros::Duration WAIT_LOOP_DURATION(0.01);
static const double INTERPOLATED_IK_PLANNING_TIME = 5.0;

void make_grasp_marker(const std::string &ns, const geometry_msgs::PoseStamped &ps, visualization_msgs::Marker &m)
{
    m.action = visualization_msgs::Marker::ADD;
    m.header = ps.header;
    m.ns = ns;
    m.id = 0;
    m.type = visualization_msgs::Marker::ARROW;
    m.pose = ps.pose;
    m.color.r = 1.0;
    m.color.a = 1.0;
    m.scale.x = 0.2;
    m.scale.y = 0.02;
    m.scale.z = 0.02;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, NODE_NAME);
    ros::NodeHandle nh;

    ros::AsyncSpinner spinner(4); // Use 4 threads
    spinner.start();

    ROS_INFO("Advertising markers");
    ros::Duration(1.0).sleep();
    ros::Publisher pub = nh.advertise<visualization_msgs::Marker>("grasp_debug", 5);

    ROS_INFO("Waiting one second for subscribers");
    ros::Duration(1.0).sleep();

    ROS_INFO("Creating planning Scene");
    boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener());
    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION, tf));
    if (planning_scene_monitor->getPlanningScene() && planning_scene_monitor->getPlanningScene()->isConfigured())
    {
        planning_scene_monitor->startWorldGeometryMonitor();
        planning_scene_monitor->startSceneMonitor();
        planning_scene_monitor->startStateMonitor();
    }
    else
    {
        ROS_ERROR("Planning scene not configured");
        return -1;
    }

    plan_execution::PlanExecution plan_execution(planning_scene_monitor);

    ROS_INFO("Waiting for robot state");
    std::vector<std::string> missing_joints;
    while(!planning_scene_monitor->getStateMonitor()->haveCompleteState(missing_joints))
    {
        ROS_INFO("Waiting for current joint states");
        for(unsigned int joint_i = 0; joint_i < missing_joints.size(); joint_i++)
            ROS_INFO("Missing joint: %s", missing_joints[joint_i].c_str());
        WAIT_LOOP_DURATION.sleep();
    }

    /* get the current gripper pose */
    planning_scene_monitor->updateFrameTransforms();
    kinematic_state::KinematicState kinematic_state = planning_scene_monitor->getPlanningScene()->getCurrentState();
    const Eigen::Affine3d *gripper_pose = kinematic_state.getFrameTransform(END_EFFECTOR_LINK_NAME);
    ROS_INFO_STREAM("Current gripper pose:" << (gripper_pose->matrix()));

    Eigen::Vector3d t = gripper_pose->translation();
    Eigen::Quaterniond q;
    q = gripper_pose->rotation();

    ROS_INFO("Current gripper trans: %f %f %f", t.x(), t.y(), t.z());
    ROS_INFO("Current gripper rot: %f %f %f %f", q.x(), q.y(), q.z(), q.w());

    /* start position is current position */
    geometry_msgs::PoseStamped ps_start, ps_end;
    ps_start.header.frame_id = "base_link";
    ps_start.header.stamp = ros::Time::now();
    ps_start.pose.position.x = t.x();
    ps_start.pose.position.y = t.y();
    ps_start.pose.position.z = t.z();
    ps_start.pose.orientation.x = q.x();
    ps_start.pose.orientation.y = q.y();
    ps_start.pose.orientation.z = q.z();
    ps_start.pose.orientation.w = q.w();

    /* end position is start position shifted a bit */
    ps_end = ps_start;
    ps_end.pose.position.x -= 0.1;

    /* publish rviz markers for the start and end poses */
    visualization_msgs::Marker m_start, m_end;
    make_grasp_marker("iik_start_pose", ps_start, m_start);
    make_grasp_marker("iik_end_pose", ps_end, m_end);
    pub.publish(m_start);
    pub.publish(m_end);

    /* create an interpolated IK planner */
    kinematics_planner::KinematicsPlanner kin_planner;
    std::vector<std::string> group_names;
    group_names.push_back(GROUP_NAME);
    kin_planner.initialize(group_names, planning_scene_monitor->getKinematicModel());

    std::map<std::string,geometry_msgs::PoseStamped> start_request;
    start_request[GROUP_NAME] = ps_start;
    std::map<std::string,geometry_msgs::PoseStamped> goal_request;
    goal_request[GROUP_NAME] = ps_end;
    moveit_msgs::Constraints path_constraints;
    moveit_msgs::RobotTrajectory robot_trajectory;
    moveit_msgs::MoveItErrorCodes error_code;

    /* create plan using interpolated IK planner */
    bool r = kin_planner.solve(start_request,
                               goal_request,
                               planning_scene_monitor->getPlanningScene(),
                               path_constraints,
                               INTERPOLATED_IK_PLANNING_TIME,
                               robot_trajectory,
                               error_code);
    if(r)
    {
        ROS_INFO("Got solution from kinematics planner");
        ROS_INFO_STREAM("Solution: " << robot_trajectory);
    }
    else
    {
        ROS_INFO("Unable to solve. Error code: %d", error_code.val);
    }

    /* execute the planned trajectory */
    ROS_INFO("Executing trajectory");
    plan_execution.getTrajectoryExecutionManager()->clear();
    if(plan_execution.getTrajectoryExecutionManager()->push(robot_trajectory))
    {
      plan_execution.getTrajectoryExecutionManager()->execute();

      /* wait for the trajectory to complete */
      moveit_controller_manager::ExecutionStatus es = plan_execution.getTrajectoryExecutionManager()->waitForExecution();
      if (es == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
        ROS_INFO("Trajectory execution succeeded");
      else
        if (es == moveit_controller_manager::ExecutionStatus::PREEMPTED)
            ROS_INFO("Trajectory execution preempted");
        else
          if (es == moveit_controller_manager::ExecutionStatus::TIMED_OUT)
              ROS_INFO("Trajectory execution timed out");
          else
              ROS_INFO("Trajectory execution control failed");
    }
    else
    {
      ROS_INFO("Failed to push trajectory");
      return -1;
    }

    planning_scene_monitor->stopSceneMonitor();

    ros::spin();
    return 0;
}
<|endoftext|>


<commit_before>//
//  redpine_module.cpp
//  Mono::Redpine
//
//  Created by Kristoffer Andersen on 27/05/15.
//
//

#include "redpine_module.h"
#include <consoles.h>
#include <mbed.h>

#include <application_context_interface.h>

using namespace mono::redpine;

/** Construct the module */
Module Module::moduleSingleton;

Module::Module()
{
    communicationInitialized = false;
    networkInitialized = false;
}

/// MARK: STATIC PUBLIC METHODS

Module* Module::Instance()
{
    return &Module::moduleSingleton;
}

bool Module::initialize(ModuleCommunication *commInterface)
{

    if (commInterface == NULL)
    {
        debug("Cannot init Redpine Module without comm. interface!\n\r");
        return false;
    }

    // Assign interface to object
    Module* self = Module::Instance();
    self->comIntf = commInterface;

    self->comIntf->resetModule();

    self->CurrentPowerState = FULL_AWAKE;

    //debug("Initializing communication interface...\n\r");
    // initalize interface
    bool success = self->comIntf->initializeInterface();

    if (!success)
    {
        debug("Initialize failed to init communication interface\n\r");
        return false;
    }

    mono::IApplicationContext::Instance->PowerManager->AppendToPowerAwareQueue(self);

    //debug("Checking bootloader state...\n\r");
    uint16_t regval = self->comIntf->readMemory(HOST_INTF_REG_OUT);
    debug("HOST_INTF_REG_OUT: 0x%x\n\r", regval);

    if ((regval & HOST_INTERACT_REG_VALID) == 0xAB00)
    {
        //debug("Module is in bootloader, load Image-I...\n\r");
        self->comIntf->InterfaceVersion = regval;
        self->comIntf->writeMemory(HOST_INTF_REG_IN, HOST_INTERACT_REG_VALID | RSI_LOAD_IMAGE_I_FW);
    }

    // we need to wait for board ready frame
    //debug("Waiting for card ready to arrive on input...\n\r");
    int timeout = 0;
    while (!self->comIntf->pollInputQueue() && timeout++ < 50)
    {
        //wait a while
        wait_ms(2);
    }

    if (timeout >= 50)
    {
        debug("Timeout: Did not receive Card ready!\n\r");
        return false;
    }

    ManagementFrame frame;
    success = self->comIntf->readManagementFrame(frame);

    if (!success)
    {
        debug("failed to read card ready!");
        return false;
    }

    self->comIntf->interruptCallback.attach<mono::redpine::Module>(self, &Module::moduleEventHandler);

    if (frame.commandId == ModuleFrame::CardReady)
    {
        //debug("Card ready received\n\r");
        self->communicationInitialized = true;
        return true;
    }
    else
    {
        debug("Initialization failed on receiving card ready\n\r");
        debug("Initialization got 0x%x, not card ready\n\r",frame.commandId);
        return false;
    }

}

bool Module::setupWifiOnly(String ssid, String passphrase, WifiSecurityModes secMode)
{
    // send "set operating mode command"
    Module *self = Module::Instance();

    if (!self->communicationInitialized)
    {
        debug("Module not initialized. You must initialize first!");
        return false;
    }

    //debug("Setting OperMode...\n\r");
    SetOperatingModeFrame *opermode = new SetOperatingModeFrame(SetOperatingModeFrame::WIFI_CLIENT_MODE);
    opermode->setDefaultConfiguration();
    opermode->commitAsync();

    //debug("Setting band...\n\r");
    BandFrame *band = new BandFrame();
    band->commitAsync();

    //debug("Initialize module RF and baseband...\n\r");
    InitFrame *init = new InitFrame();
    init->commitAsync();

    //debug("Scanning for networks...\n\r");
    ScanFrame *scan = new ScanFrame();
    scan->commitAsync();

    //debug("Connecting to %s\n\r",ssid);
    JoinFrame *join = new JoinFrame(ssid, passphrase, secMode);
    join->commitAsync();

    //debug("Getting IP address from DHCP...\n\r");
    SetIpParametersFrame *ipparam = new SetIpParametersFrame();
    if (self->staticIPHandler)
    {
        ipparam->dhcpMode = SetIpParametersFrame::STATIC_IP;
        StaticIPParams params;
        self->staticIPHandler.call(&params);
        memcpy(ipparam->ipAddress, params.ipAddress, 4);
        memcpy(ipparam->netmask, params.netmask, 4);
        memcpy(ipparam->gateway, params.gateway, 4);
    }

    ipparam->setCompletionCallback<Module>(Module::Instance(), &Module::onNetworkReady);

    ipparam->commitAsync();

    return true;
}

bool Module::IsNetworkReady()
{
    return Module::Instance()->networkInitialized;
}

/// MARK: PRIVATE METHODS

void Module::moduleEventHandler()
{

    if (CurrentPowerState & (LOW_SLEEP | ULTRA_LOW_SLEEP))
        handleSleepWakeUp();
    else
    {

        if (comIntf->pollInputQueue())
        {
            //handle a response for any pending request
            ManagementFrame *respFrame = responseFrameQueue.Peek();
            if (respFrame == NULL)
            {
                debug("nothing on request queue!\n\r");
                ManagementFrame frame;
                bool success = comIntf->readManagementFrame(frame);
                if (!success)
                {
                    debug("Failed to read unexcepted mgmt frame\n\r");
                }
            }
            else
            {
                debug("resp frame cmd id: 0x%x\n\r",respFrame->commandId);
                //memdump(reqFrame, sizeof(mono::redpine::HttpGetFrame));

                bool success = this->comIntf->readManagementFrameResponse(*respFrame);

                if (!success) {
                    responseFrameQueue.Remove(respFrame);
                    debug("failed to handle incoming response for resp queue head\n\r");
                    respFrame->status = 1;
                    respFrame->triggerCompletionHandler();
                }
                else if (respFrame->lastResponseParsed)
                {
                    //debug("Frame (0x%x): last response parsed!\n\r",respFrame->commandId);
                    responseFrameQueue.Remove(respFrame);
                    respFrame->triggerCompletionHandler();

                    if (respFrame->autoReleaseWhenParsed)
                    {
                        //debug("deleting resp frame 0x%x!\n\r",respFrame->commandId);
                        delete respFrame;
                    }
                    else
                    {
                        debug("leaving frame 0x%x\n\r",respFrame->commandId);
                    }

                }

            }

        }

        // if no response is pending, send new request
        if (responseFrameQueue.Length() == 0 && requestFrameQueue.Length() > 0)
        {
            ManagementFrame *request = requestFrameQueue.Dequeue();
            //debug("Sending Mgmt request 0x%x\n\r",request->commandId);
            bool success = request->writeFrame();

            if (!success)
            {
                debug("Failed to send MgmtFrame (0x%x) to module\n\r",request->commandId);

                if (request->autoReleaseWhenParsed)
                    delete request;

                return;
            }

            responseFrameQueue.Enqueue(request);
        }
    }
}

void Module::onNetworkReady(ManagementFrame::FrameCompletionData *data)
{
    if (data->Context->commandId == ModuleFrame::SetIPParameters)
    {
        SetIpParametersFrame *ip = (SetIpParametersFrame*) data->Context;

        memcpy(this->ipAddress, ip->ipAddress, 4);
        memcpy(this->netmask, ip->netmask, 4);
        memcpy(this->gateway, ip->gateway, 4);
        memcpy(this->macAddress, ip->macAddress, 6);

        debug("Network Ready!\n\r");
        networkInitialized = true;

        if (networkReadyHandler) {
            networkReadyHandler.call();
        }
    }
}

void Module::handleSleepWakeUp()
{

    if (!(CurrentPowerState & (ULTRA_LOW_SLEEP | LOW_SLEEP)))
    {
        debug("Module not in sleep!\n\r");
        return;
    }

    // if interface was initialized, we are out of sleep
    if (comIntf->initializeInterface())
        CurrentPowerState = FULL_AWAKE;

    if (comIntf->pollInputQueue())
    {
        ManagementFrame frame;
        comIntf->readManagementFrame(frame);

        if (frame.commandId == ManagementFrame::WakeFromSleep)
        {
            debug("putting to sleep again!\n\r");
            ManagementFrame sleepAgain(ManagementFrame::PowerSaveACK);
            sleepAgain.commit();

        }
        else
        {
            debug("Unkown commandID for handleSleepWakeUp: 0x%x\n\r",frame.commandId);
        }
    }
    else
    {
        debug("nothing in input poll queue\n\r");
    }
}

void Module::onSystemPowerOnReset()
{

}

void Module::onSystemEnterSleep()
{
    //comIntf->resetModule();
}

void Module::onSystemWakeFromSleep()
{

}

void Module::OnSystemBatteryLow()
{

}

<commit_msg>Wire module handles failed request<commit_after>//
//  redpine_module.cpp
//  Mono::Redpine
//
//  Created by Kristoffer Andersen on 27/05/15.
//
//

#include "redpine_module.h"
#include <consoles.h>
#include <mbed.h>

#include <application_context_interface.h>

using namespace mono::redpine;

/** Construct the module */
Module Module::moduleSingleton;

Module::Module()
{
    communicationInitialized = false;
    networkInitialized = false;
}

/// MARK: STATIC PUBLIC METHODS

Module* Module::Instance()
{
    return &Module::moduleSingleton;
}

bool Module::initialize(ModuleCommunication *commInterface)
{

    if (commInterface == NULL)
    {
        debug("Cannot init Redpine Module without comm. interface!\n\r");
        return false;
    }

    // Assign interface to object
    Module* self = Module::Instance();
    self->comIntf = commInterface;

    self->comIntf->resetModule();

    self->CurrentPowerState = FULL_AWAKE;

    //debug("Initializing communication interface...\n\r");
    // initalize interface
    bool success = self->comIntf->initializeInterface();

    if (!success)
    {
        debug("Initialize failed to init communication interface\n\r");
        return false;
    }

    mono::IApplicationContext::Instance->PowerManager->AppendToPowerAwareQueue(self);

    //debug("Checking bootloader state...\n\r");
    uint16_t regval = self->comIntf->readMemory(HOST_INTF_REG_OUT);
    debug("HOST_INTF_REG_OUT: 0x%x\n\r", regval);

    if ((regval & HOST_INTERACT_REG_VALID) == 0xAB00)
    {
        //debug("Module is in bootloader, load Image-I...\n\r");
        self->comIntf->InterfaceVersion = regval;
        self->comIntf->writeMemory(HOST_INTF_REG_IN, HOST_INTERACT_REG_VALID | RSI_LOAD_IMAGE_I_FW);
    }

    // we need to wait for board ready frame
    //debug("Waiting for card ready to arrive on input...\n\r");
    int timeout = 0;
    while (!self->comIntf->pollInputQueue() && timeout++ < 50)
    {
        //wait a while
        wait_ms(2);
    }

    if (timeout >= 50)
    {
        debug("Timeout: Did not receive Card ready!\n\r");
        return false;
    }

    ManagementFrame frame;
    success = self->comIntf->readManagementFrame(frame);

    if (!success)
    {
        debug("failed to read card ready!");
        return false;
    }

    self->comIntf->interruptCallback.attach<mono::redpine::Module>(self, &Module::moduleEventHandler);

    if (frame.commandId == ModuleFrame::CardReady)
    {
        //debug("Card ready received\n\r");
        self->communicationInitialized = true;
        return true;
    }
    else
    {
        debug("Initialization failed on receiving card ready\n\r");
        debug("Initialization got 0x%x, not card ready\n\r",frame.commandId);
        return false;
    }

}

bool Module::setupWifiOnly(String ssid, String passphrase, WifiSecurityModes secMode)
{
    // send "set operating mode command"
    Module *self = Module::Instance();

    if (!self->communicationInitialized)
    {
        debug("Module not initialized. You must initialize first!");
        return false;
    }

    //debug("Setting OperMode...\n\r");
    SetOperatingModeFrame *opermode = new SetOperatingModeFrame(SetOperatingModeFrame::WIFI_CLIENT_MODE);
    opermode->setDefaultConfiguration();
    opermode->commitAsync();

    //debug("Setting band...\n\r");
    BandFrame *band = new BandFrame();
    band->commitAsync();

    //debug("Initialize module RF and baseband...\n\r");
    InitFrame *init = new InitFrame();
    init->commitAsync();

    //debug("Scanning for networks...\n\r");
    ScanFrame *scan = new ScanFrame();
    scan->commitAsync();

    //debug("Connecting to %s\n\r",ssid);
    JoinFrame *join = new JoinFrame(ssid, passphrase, secMode);
    join->commitAsync();

    //debug("Getting IP address from DHCP...\n\r");
    SetIpParametersFrame *ipparam = new SetIpParametersFrame();
    if (self->staticIPHandler)
    {
        ipparam->dhcpMode = SetIpParametersFrame::STATIC_IP;
        StaticIPParams params;
        self->staticIPHandler.call(&params);
        memcpy(ipparam->ipAddress, params.ipAddress, 4);
        memcpy(ipparam->netmask, params.netmask, 4);
        memcpy(ipparam->gateway, params.gateway, 4);
    }

    ipparam->setCompletionCallback<Module>(Module::Instance(), &Module::onNetworkReady);

    ipparam->commitAsync();

    return true;
}

bool Module::IsNetworkReady()
{
    return Module::Instance()->networkInitialized;
}

/// MARK: PRIVATE METHODS

void Module::moduleEventHandler()
{

    if (CurrentPowerState & (LOW_SLEEP | ULTRA_LOW_SLEEP))
        handleSleepWakeUp();
    else
    {

        if (comIntf->pollInputQueue())
        {
            //handle a response for any pending request
            ManagementFrame *respFrame = responseFrameQueue.Peek();
            if (respFrame == NULL)
            {
                debug("nothing on request queue!\n\r");
                ManagementFrame frame;
                bool success = comIntf->readManagementFrame(frame);
                if (!success)
                {
                    debug("Failed to read unexcepted mgmt frame\n\r");
                }
            }
            else
            {
                debug("resp frame cmd id: 0x%x\n\r",respFrame->commandId);
                //memdump(reqFrame, sizeof(mono::redpine::HttpGetFrame));

                bool success = this->comIntf->readManagementFrameResponse(*respFrame);

                if (!success) {
                    responseFrameQueue.Remove(respFrame);
                    debug("failed to handle incoming response for resp queue head\n\r");
                    respFrame->status = 1;
                    respFrame->triggerCompletionHandler();
                }
                else if (respFrame->lastResponseParsed)
                {
                    //debug("Frame (0x%x): last response parsed!\n\r",respFrame->commandId);
                    responseFrameQueue.Remove(respFrame);
                    respFrame->triggerCompletionHandler();

                    if (respFrame->autoReleaseWhenParsed)
                    {
                        //debug("deleting resp frame 0x%x!\n\r",respFrame->commandId);
                        delete respFrame;
                    }
                    else
                    {
                        debug("leaving frame 0x%x\n\r",respFrame->commandId);
                    }

                }

            }

        }

        // if no response is pending, send new request
        if (responseFrameQueue.Length() == 0 && requestFrameQueue.Length() > 0)
        {
            ManagementFrame *request = requestFrameQueue.Dequeue();
            //debug("Sending Mgmt request 0x%x\n\r",request->commandId);
            bool success = request->writeFrame();

            if (!success)
            {
                debug("Failed to send MgmtFrame (0x%x) to module\n\r",request->commandId);
                request->status = 1;
                request->triggerCompletionHandler();

                if (request->autoReleaseWhenParsed)
                    delete request;

                return;
            }

            responseFrameQueue.Enqueue(request);
        }
    }
}

void Module::onNetworkReady(ManagementFrame::FrameCompletionData *data)
{
    if (data->Context->commandId == ModuleFrame::SetIPParameters)
    {
        SetIpParametersFrame *ip = (SetIpParametersFrame*) data->Context;

        memcpy(this->ipAddress, ip->ipAddress, 4);
        memcpy(this->netmask, ip->netmask, 4);
        memcpy(this->gateway, ip->gateway, 4);
        memcpy(this->macAddress, ip->macAddress, 6);

        debug("Network Ready!\n\r");
        networkInitialized = true;

        if (networkReadyHandler) {
            networkReadyHandler.call();
        }
    }
}

void Module::handleSleepWakeUp()
{

    if (!(CurrentPowerState & (ULTRA_LOW_SLEEP | LOW_SLEEP)))
    {
        debug("Module not in sleep!\n\r");
        return;
    }

    // if interface was initialized, we are out of sleep
    if (comIntf->initializeInterface())
        CurrentPowerState = FULL_AWAKE;

    if (comIntf->pollInputQueue())
    {
        ManagementFrame frame;
        comIntf->readManagementFrame(frame);

        if (frame.commandId == ManagementFrame::WakeFromSleep)
        {
            debug("putting to sleep again!\n\r");
            ManagementFrame sleepAgain(ManagementFrame::PowerSaveACK);
            sleepAgain.commit();

        }
        else
        {
            debug("Unkown commandID for handleSleepWakeUp: 0x%x\n\r",frame.commandId);
        }
    }
    else
    {
        debug("nothing in input poll queue\n\r");
    }
}

void Module::onSystemPowerOnReset()
{

}

void Module::onSystemEnterSleep()
{
    //comIntf->resetModule();
}

void Module::onSystemWakeFromSleep()
{

}

void Module::OnSystemBatteryLow()
{

}

<|endoftext|>


<commit_before>#ifndef ITER_SLIDING_WINDOW_HPP_
#define ITER_SLIDING_WINDOW_HPP_

#include "iterbase.hpp"

#include <deque>
#include <utility>
#include <iterator>

namespace iter {
    template <typename Container>
    class SlidingWindow;

    template <typename Container>
    SlidingWindow<Container> sliding_window(Container&&, std::size_t);

    template <typename T>
    SlidingWindow<std::initializer_list<T>> sliding_window(
            std::initializer_list<T>, std::size_t);

    template <typename Container>
    class SlidingWindow {
        private: Container container;
            std::size_t window_size;

            friend SlidingWindow sliding_window<Container>(
                    Container&&, std::size_t);

            template <typename T>
            friend SlidingWindow<std::initializer_list<T>> sliding_window(
                    std::initializer_list<T>, std::size_t);

            SlidingWindow(Container container, std::size_t win_sz)
                : container(std::forward<Container>(container)),
                window_size{win_sz}
            { }

            using DerefVec = std::deque<collection_item_type<Container>>;
        public:

            class Iterator
                : public std::iterator<std::input_iterator_tag, DerefVec>
            {
                private:
                    iterator_type<Container> sub_iter;
                    DerefVec window;

                public:
                    Iterator(const iterator_type<Container>& in_iter,
                            const iterator_type<Container>& in_end,
                             std::size_t window_sz)
                        : sub_iter(in_iter)
                    {
                        std::size_t i{0};
                        while (i < window_sz && this->sub_iter != in_end) {
                            this->window.push_back(*this->sub_iter);
                            ++i;
                            if (i != window_sz) ++this->sub_iter;
                        }
                    }

                    bool operator!=(const Iterator& other) const {
                        return this->sub_iter != other.sub_iter;
                    }

                    bool operator==(const Iterator& other) const {
                        return !(*this != other);
                    }

                    DerefVec& operator*() {
                        return this->window;
                    }

                    Iterator& operator++() {
                        ++this->sub_iter;
                        this->window.pop_front();
                        this->window.push_back(*this->sub_iter);
                        return *this;
                    }

                    Iterator operator++(int) {
                        auto ret = *this;
                        ++*this;
                        return ret;
                    }
            };

            Iterator begin() {
                return {std::begin(container),
                    std::end(container),
                    window_size};
            }

            Iterator end() {
                return {std::end(container),
                    std::end(container),
                    window_size};
            }
    };

    template <typename Container>
    SlidingWindow<Container> sliding_window(
            Container&& container, std::size_t window_size) {
        return {std::forward<Container>(container), window_size};
    }

    template <typename T>
    SlidingWindow<std::initializer_list<T>> sliding_window(
            std::initializer_list<T> il, std::size_t window_size) {
        return {il, window_size};
    }
}

#endif
<commit_msg>handles window size of 0<commit_after>#ifndef ITER_SLIDING_WINDOW_HPP_
#define ITER_SLIDING_WINDOW_HPP_

#include "iterbase.hpp"

#include <deque>
#include <utility>
#include <iterator>

namespace iter {
    template <typename Container>
    class SlidingWindow;

    template <typename Container>
    SlidingWindow<Container> sliding_window(Container&&, std::size_t);

    template <typename T>
    SlidingWindow<std::initializer_list<T>> sliding_window(
            std::initializer_list<T>, std::size_t);

    template <typename Container>
    class SlidingWindow {
        private: Container container;
            std::size_t window_size;

            friend SlidingWindow sliding_window<Container>(
                    Container&&, std::size_t);

            template <typename T>
            friend SlidingWindow<std::initializer_list<T>> sliding_window(
                    std::initializer_list<T>, std::size_t);

            SlidingWindow(Container container, std::size_t win_sz)
                : container(std::forward<Container>(container)),
                window_size{win_sz}
            { }

            using DerefVec = std::deque<collection_item_type<Container>>;
        public:

            class Iterator
                : public std::iterator<std::input_iterator_tag, DerefVec>
            {
                private:
                    iterator_type<Container> sub_iter;
                    DerefVec window;

                public:
                    Iterator(const iterator_type<Container>& in_iter,
                            const iterator_type<Container>& in_end,
                             std::size_t window_sz)
                        : sub_iter(in_iter)
                    {
                        std::size_t i{0};
                        while (i < window_sz && this->sub_iter != in_end) {
                            this->window.push_back(*this->sub_iter);
                            ++i;
                            if (i != window_sz) ++this->sub_iter;
                        }
                    }

                    bool operator!=(const Iterator& other) const {
                        return this->sub_iter != other.sub_iter;
                    }

                    bool operator==(const Iterator& other) const {
                        return !(*this != other);
                    }

                    DerefVec& operator*() {
                        return this->window;
                    }

                    Iterator& operator++() {
                        ++this->sub_iter;
                        this->window.pop_front();
                        this->window.push_back(*this->sub_iter);
                        return *this;
                    }

                    Iterator operator++(int) {
                        auto ret = *this;
                        ++*this;
                        return ret;
                    }
            };

            Iterator begin() {
                return {
                    (this->window_size != 0 ?
                     std::begin(this->container)
                     : std::end(this->container)),
                    std::end(this->container),
                    this->window_size};
            }

            Iterator end() {
                return {std::end(this->container),
                    std::end(this->container),
                    this->window_size};
            }
    };

    template <typename Container>
    SlidingWindow<Container> sliding_window(
            Container&& container, std::size_t window_size) {
        return {std::forward<Container>(container), window_size};
    }

    template <typename T>
    SlidingWindow<std::initializer_list<T>> sliding_window(
            std::initializer_list<T> il, std::size_t window_size) {
        return {il, window_size};
    }
}

#endif
<|endoftext|>


<commit_before>#include <iostream>
#include <string>
#include <limits>

using namespace std;

int main()
{
	string userInput;
	int trackNumber = 0;

	do
	{
		cout << "Pls enter track number: ";
		//getline (cin, userInput);
		cin >> trackNumber;
		if (cin.fail())
		{
			cin.clear();
			cin.ignore(numeric_limits<streamsize>::max(), '\n');
			trackNumber = 0;
			continue;
		}

		cout << "You entered: " << trackNumber << endl;
		cin.ignore();
	} while (trackNumber < 1 || trackNumber > 4);

	return 0;
}
<commit_msg>fixed stuff<commit_after>#include <iostream>
#include <string>
#include <limits>

using namespace std;

int main()
{
	int trackNumber = 0;

	do
	{
		cout << "Pls enter track number: ";
		cin >> noskipws >> trackNumber;

		if (cin.fail())
		{
			cout << "What a load of rubbish\n";
			cin.clear();
			cin.ignore(numeric_limits<streamsize>::max(), '\n');
			trackNumber = 0;
			continue;
		}

		cout << "You entered: " << trackNumber << endl;
		cin.ignore();
	} while (trackNumber < 1 || trackNumber > 4);

	return 0;
}
<|endoftext|>


<commit_before>//===-- UriParser.cpp -------------------------------------------*- C++ -*-===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "lldb/Utility/UriParser.h"

#include <string>

#include <stdint.h>
#include <tuple>

using namespace lldb_private;

//----------------------------------------------------------------------
// UriParser::Parse
//----------------------------------------------------------------------
bool UriParser::Parse(llvm::StringRef uri, llvm::StringRef &scheme,
                      llvm::StringRef &hostname, int &port,
                      llvm::StringRef &path) {
  llvm::StringRef tmp_scheme, tmp_hostname, tmp_port, tmp_path;

  const llvm::StringRef kSchemeSep("://");
  auto pos = uri.find(kSchemeSep);
  if (pos == std::string::npos)
    return false;

  // Extract path.
  tmp_scheme = uri.substr(0, pos);
  auto host_pos = pos + kSchemeSep.size();
  auto path_pos = uri.find('/', host_pos);
  if (path_pos != std::string::npos)
    tmp_path = uri.substr(path_pos);
  else
    tmp_path = "/";

  auto host_port = uri.substr(
      host_pos,
      ((path_pos != std::string::npos) ? path_pos : uri.size()) - host_pos);

  // Extract hostname
  if (host_port[0] == '[') {
    // hostname is enclosed with square brackets.
    pos = host_port.find(']');
    if (pos == std::string::npos)
      return false;

    tmp_hostname = host_port.substr(1, pos - 1);
    host_port = host_port.drop_front(pos + 1);
    if (!host_port.empty() && !host_port.consume_front(":"))
      return false;
  } else {
    std::tie(tmp_hostname, host_port) = host_port.split(':');
  }

  // Extract port
  if (!host_port.empty()) {
    uint16_t port_value = 0;
    if (host_port.getAsInteger(0, port_value))
      return false;
    port = port_value;
  } else
    port = -1;

  scheme = tmp_scheme;
  hostname = tmp_hostname;
  path = tmp_path;
  return true;
}
<commit_msg>Don't crash when hostname is empty. StringRef will assert and kill your program.<commit_after>//===-- UriParser.cpp -------------------------------------------*- C++ -*-===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "lldb/Utility/UriParser.h"

#include <string>

#include <stdint.h>
#include <tuple>

using namespace lldb_private;

//----------------------------------------------------------------------
// UriParser::Parse
//----------------------------------------------------------------------
bool UriParser::Parse(llvm::StringRef uri, llvm::StringRef &scheme,
                      llvm::StringRef &hostname, int &port,
                      llvm::StringRef &path) {
  llvm::StringRef tmp_scheme, tmp_hostname, tmp_port, tmp_path;

  const llvm::StringRef kSchemeSep("://");
  auto pos = uri.find(kSchemeSep);
  if (pos == std::string::npos)
    return false;

  // Extract path.
  tmp_scheme = uri.substr(0, pos);
  auto host_pos = pos + kSchemeSep.size();
  auto path_pos = uri.find('/', host_pos);
  if (path_pos != std::string::npos)
    tmp_path = uri.substr(path_pos);
  else
    tmp_path = "/";

  auto host_port = uri.substr(
      host_pos,
      ((path_pos != std::string::npos) ? path_pos : uri.size()) - host_pos);

  // Extract hostname
  if (!host_port.empty() && host_port[0] == '[') {
    // hostname is enclosed with square brackets.
    pos = host_port.find(']');
    if (pos == std::string::npos)
      return false;

    tmp_hostname = host_port.substr(1, pos - 1);
    host_port = host_port.drop_front(pos + 1);
    if (!host_port.empty() && !host_port.consume_front(":"))
      return false;
  } else {
    std::tie(tmp_hostname, host_port) = host_port.split(':');
  }

  // Extract port
  if (!host_port.empty()) {
    uint16_t port_value = 0;
    if (host_port.getAsInteger(0, port_value))
      return false;
    port = port_value;
  } else
    port = -1;

  scheme = tmp_scheme;
  hostname = tmp_hostname;
  path = tmp_path;
  return true;
}
<|endoftext|>


<commit_before>// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: threads.C,v 1.41.16.2 2007/05/10 20:15:09 amoll Exp $
//

#include <BALL/VIEW/KERNEL/threads.h>
#include <BALL/VIEW/KERNEL/mainControl.h>
#include <BALL/VIEW/KERNEL/message.h>
#include <BALL/VIEW/KERNEL/representation.h>
#include <BALL/VIEW/DIALOGS/FDPBDialog.h>
#include <BALL/VIEW/DATATYPE/standardDatasets.h>

#include <BALL/MOLMEC/COMMON/forceField.h>
#include <BALL/MOLMEC/MINIMIZATION/energyMinimizer.h>
#include <BALL/MOLMEC/MDSIMULATION/molecularDynamics.h>
#include <BALL/MOLMEC/COMMON/snapShotManager.h>

#include <BALL/STRUCTURE/DOCKING/dockingAlgorithm.h>
#include <BALL/FORMAT/DCDFile.h>

#include <QtGui/qapplication.h>

namespace BALL
{
	namespace VIEW
	{

		BALLThread::BALLThread()
			: QThread(),
				main_control_(0),
				composite_(0)
		{
		}

		void BALLThread::output_(const String& string, bool important)
		{
			LogEvent* su = new LogEvent;
			su->setMessage(string + String("\n"));
			su->setImportant(important);
			qApp->postEvent(main_control_, su);  // Qt will delete it when done
		}

		void BALLThread::waitForUpdateOfRepresentations_()
		{
			RepresentationManager& pm = main_control_->getRepresentationManager();
			while (!main_control_->stopedSimulation() &&
					   (pm.still_to_notify_ || pm.updateRunning()))

			{
				msleep(10);
			}
		}

		void BALLThread::updateStructure_()
		{
			RepresentationManager& pm = main_control_->getRepresentationManager();
			pm.still_to_notify_ = true;
			// notify MainControl to update all Representations for the Composite
			sendMessage_(new CompositeMessage(*composite_,
																				CompositeMessage::CHANGED_COMPOSITE,
																				true));
		}

		void BALLThread::sendMessage_(Message* msg)
		{
			// Qt will delete the MessageEvent when done
			qApp->postEvent(main_control_, new MessageEvent(msg));
		}

		// ================================== FetchHTMLThread ===============================

		FetchHTMLThread::FetchHTMLThread()
			throw()
			: BALLThread(),
				file_name_("")
		{
		}

		void FetchHTMLThread::setURL(const String& url)
			throw()
		{
			url_ = url;
		}

		void FetchHTMLThread::run()
		{
			if (main_control_ == 0) return;

			if (url_ == "")
			{
				output_("Invalid Address " + url_ + " in " + String(__FILE__) + ":" + String(__LINE__), true);
				return;
			}

			if (main_control_ != 0)
			{
				tcp_.setProxy(main_control_->getProxy(), main_control_->getProxyPort());
			}

			try
			{
				if (file_name_ != "")
				{
					File f(file_name_, std::ios::out);
					if (!f.isOpen())
					{
						output_(String("Could not open file ") + file_name_ + " for output!");
						return;
					}
					tcp_.set(f, url_);
					// dont move the transfer() call away from here!
					tcp_.transfer();
				}
				else
				{
					char c;
					stream_.get(c);

					while (stream_.gcount() > 0)
					{
						stream_.get(c);
					}

					stream_.clear();

					tcp_.set(stream_, url_);
					// dont move the transfer() call away from here!
					tcp_.transfer();
				}
			}
			catch(...)
			{
				output_(String("Exception in ") + String(__FILE__) + ": " + String(__LINE__), true);
			}
		}

		// ==========================================

		UpdateRepresentationThread::UpdateRepresentationThread()
			throw()
			: BALLThread(),
				rep_(0)
		{
			setTerminationEnabled(true);
		}

		void UpdateRepresentationThread::run()
		{
			if (main_control_ == 0) return;

			RepresentationManager& pm = main_control_->getRepresentationManager();
			Representation* rep = 0;
			while (!main_control_->isAboutToQuit())
			{
				if (!main_control_->useMultithreading())
				{
					msleep(50);
					continue;
				}

				rep = pm.popRepresentationToUpdate();
				if (rep == 0)
				{
					msleep(10);
					continue;
				}

				rep->update_();

				sendMessage_(new RepresentationMessage(*rep, RepresentationMessage::FINISHED_UPDATE));
			}
		}

		// ==========================================

		SimulationThread::SimulationThread()
			: BALLThread(),
				steps_between_updates_(0),
				dcd_file_(0)
		{
			setTerminationEnabled(true);
		}

		void SimulationThread::exportSceneToPNG_()
		{
			if (main_control_->stopedSimulation()) return;

			sendMessage_(new SceneMessage(SceneMessage::EXPORT_PNG));
		}

		void SimulationThread::finish_()
		{
			if (dcd_file_ != 0)
			{
				dcd_file_->close();
				String filename = dcd_file_->getName();
				delete dcd_file_;
				dcd_file_ = 0;
				// we will reopen the file to prevent problems when a user runs an other sim
				SnapShotManagerDataset* set = new SnapShotManagerDataset;
				set->setName(filename);
				set->setType(TrajectoryController::type);
				set->setComposite(getComposite());
				SnapShotManager* manager = new SnapShotManager((System*)getComposite(), 0,
																					new DCDFile(filename, std::ios::in));
				set->setData(manager);
				sendMessage_(new DatasetMessage(set, DatasetMessage::ADD));
			}
			sendMessage_(new FinishedSimulationMessage);
		}

		// =====================================================================
		void EnergyMinimizerThread::run()
		{
			if (main_control_ == 0) return;

			try
			{
				if (minimizer_ == 0 															||
						minimizer_->getForceField() == 0 							||
						minimizer_->getForceField()->getSystem() == 0 ||
						main_control_ == 0)
				{
					throw Exception::NullPointer(__FILE__, __LINE__);
				}

				ForceField& ff = *minimizer_->getForceField();
				bool ok = true;
				bool converged = false;
				// iterate until done and refresh the screen every "steps" iterations
				while (!main_control_->stopedSimulation() &&
								minimizer_->getNumberOfIterations() < minimizer_->getMaxNumberOfIterations() &&
								!converged && ok)
				{
					converged = minimizer_->minimize(steps_between_updates_, true);
					ok = !minimizer_->wasAborted();

					updateStructure_();
					waitForUpdateOfRepresentations_();

					QString message;
					message.sprintf("Iteration %d: energy = %f kJ/mol, RMS gradient = %f kJ/mol A",
													minimizer_->getNumberOfIterations(),
													ff.getEnergy(), ff.getRMSGradient());
					output_(ascii(message));
				}

				updateStructure_();

				output_(ff.getResults());
				output_("final RMS gradient    : " + String(ff.getRMSGradient()) + " kJ/(mol A)   after "
								+ String(minimizer_->getNumberOfIterations()) + " iterations\n",
								true);

				if (converged) output_("converged!");
				if (!ok) output_("aborted!");
				if (minimizer_->getNumberOfIterations() == minimizer_->getMaxNumberOfIterations()) output_("max number of iterations reached!");

				finish_();

				if (!ok)
				{
					output_("Aborted minimization because convergence could not be reached. Try to restart the minimization.", true);
					return;
				}
			}
			catch(Exception::GeneralException e)
			{
				delete dcd_file_;
				dcd_file_ = 0;

				String txt = String("Exception was thrown during minimization: ") + __FILE__ + ": " + String(__LINE__) + " :\n"
											+ e.getMessage();
				output_(txt, true);

				finish_();
			}
		}

		EnergyMinimizerThread::EnergyMinimizerThread()
			: SimulationThread(),
				minimizer_(0)
		{
		}

		EnergyMinimizerThread::~EnergyMinimizerThread()
		{
			if (minimizer_ != 0)
			{
				delete minimizer_;
			}
		}

		void EnergyMinimizerThread::setEnergyMinimizer(EnergyMinimizer* minimizer)
		{
			if (minimizer_ != 0)
			{
				delete minimizer_;
			}
			minimizer_ = minimizer;
		}

		// =====================================================
		void MDSimulationThread::run()
			throw(Exception::NullPointer)
		{
			if (main_control_ == 0) return;

			try
			{
				if (md_ == 0 ||
						md_->getForceField() == 0 ||
						md_->getForceField()->getSystem() == 0 ||
						main_control_ == 0)
				{
					throw Exception::NullPointer(__FILE__, __LINE__);
				}
				ForceField& ff = *md_->getForceField();

				SnapShotManager manager(ff.getSystem(), &ff, dcd_file_);
				manager.setFlushToDiskFrequency(10);
				bool ok = true;

				// iterate until done and refresh the screen every "steps" iterations
				while (ok &&
							 md_->getNumberOfIterations() < steps_ &&
							 !main_control_->stopedSimulation())
				{
					ok = md_->simulateIterations(steps_between_updates_, true);
					updateStructure_();

					waitForUpdateOfRepresentations_();

					QString message;
					message.sprintf("Iteration %d: energy = %f kJ/mol, RMS gradient = %f kJ/mol A",
													md_->getNumberOfIterations(), ff.getEnergy(),
													ff.getRMSGradient());
					output_(ascii(message));

					if (save_images_) exportSceneToPNG_();
					if (dcd_file_) 		manager.takeSnapShot();
				}

				if (dcd_file_) manager.flushToDisk();

 				output_(ff.getResults());
				output_("final RMS gradient    : " + String(ff.getRMSGradient()) + " kJ/(mol A)   after "
								+ String(md_->getNumberOfIterations()) + " iterations\n",
								true);

				if (!ok)
				{
					output_("Simulation aborted to to strange energy values.", true);
				}

				finish_();
			}
			catch(Exception::GeneralException e)
			{
				String txt = String("Exception was thrown during MD simulation: ")
											+ __FILE__ + ": " + String(__LINE__) + " \n" + e.getMessage();
				output_(txt, true);

				if (dcd_file_ != 0)
				{
					dcd_file_->close();
					delete dcd_file_;
					dcd_file_ = 0;
				}

				finish_();
			}
		}

		MDSimulationThread::MDSimulationThread()
			: SimulationThread(),
				md_(0),
				save_images_(false)
		{
		}

		MDSimulationThread::~MDSimulationThread()
		{
			if (md_ != 0) delete md_;
		}

		void MDSimulationThread::setMolecularDynamics(MolecularDynamics* md)
		{
			if (md_ != 0) delete md_;

			md_ = md;
		}

		// =================================================0

		CalculateFDPBThread::CalculateFDPBThread()
			throw()
			: BALLThread(),
				dialog_(0)
		{}

		void CalculateFDPBThread::run()
		{
			if (dialog_ == 0) return;
			dialog_->calculate_();
		}

		// =========================== implementation of class DockingThread ================

		///
		DockingThread::DockingThread()
			throw()
			: BALLThread(),
				dock_alg_(0)
		{}

		// Copy constructor.
		DockingThread::DockingThread(const DockingThread& dock_thread)
			throw()
			: BALLThread(),
				dock_alg_(dock_thread.dock_alg_)
		{}

		///
		DockingThread::~DockingThread()
			throw()
		{
			output_("delete thread", true);

			// docking algorithm is deleted in DockingController
		}

		// Assignment operator
		const DockingThread& DockingThread::operator =(const DockingThread& dock_thread)
			throw()
		{
			if (&dock_thread != this)
			{
				dock_alg_ = dock_thread.dock_alg_;
			}
			return *this;
		}

		//
		void DockingThread::setDockingAlgorithm(DockingAlgorithm* dock_alg)
			throw()
		{
			dock_alg_ = dock_alg;
		}

		///
		void DockingThread::run()
			throw(Exception::NullPointer)
		{
				if (dock_alg_ == 0 ||
						main_control_ == 0)
				{
					throw Exception::NullPointer(__FILE__, __LINE__);
				}

				output_("starting docking...", true);

				dock_alg_->start();

				DockingFinishedMessage* msg = new DockingFinishedMessage(dock_alg_->wasAborted());
				// conformation set is deleted in DockResult
				msg->setConformationSet(new ConformationSet(dock_alg_->getConformationSet()));
				sendMessage_(msg);

				output_("Docking finished.", true);
		}

		// =================================================0

	} // namespace VIEW
} // namespace BALL

<commit_msg>no message<commit_after>// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: threads.C,v 1.41.16.3 2007/05/28 13:35:28 amoll Exp $
//

#include <BALL/VIEW/KERNEL/threads.h>
#include <BALL/VIEW/KERNEL/mainControl.h>
#include <BALL/VIEW/KERNEL/message.h>
#include <BALL/VIEW/KERNEL/representation.h>
#include <BALL/VIEW/DIALOGS/FDPBDialog.h>
#include <BALL/VIEW/DATATYPE/standardDatasets.h>

#include <BALL/MOLMEC/COMMON/forceField.h>
#include <BALL/MOLMEC/MINIMIZATION/energyMinimizer.h>
#include <BALL/MOLMEC/MDSIMULATION/molecularDynamics.h>
#include <BALL/MOLMEC/COMMON/snapShotManager.h>

#include <BALL/STRUCTURE/DOCKING/dockingAlgorithm.h>
#include <BALL/FORMAT/DCDFile.h>

#include <QtGui/qapplication.h>

namespace BALL
{
	namespace VIEW
	{

		BALLThread::BALLThread()
			: QThread(),
				main_control_(0),
				composite_(0)
		{
		}

		void BALLThread::output_(const String& string, bool important)
		{
			LogEvent* su = new LogEvent;
			su->setMessage(string + String("\n"));
			su->setImportant(important);
			qApp->postEvent(main_control_, su);  // Qt will delete it when done
		}

		void BALLThread::waitForUpdateOfRepresentations_()
		{
			RepresentationManager& pm = main_control_->getRepresentationManager();
			while (!main_control_->stopedSimulation() &&
					   (pm.still_to_notify_ || pm.updateRunning()))

			{
				msleep(10);
			}
		}

		void BALLThread::updateStructure_()
		{
			RepresentationManager& pm = main_control_->getRepresentationManager();
			pm.still_to_notify_ = true;
			// notify MainControl to update all Representations for the Composite
			sendMessage_(new CompositeMessage(*composite_,
																				CompositeMessage::CHANGED_COMPOSITE,
																				true));
		}

		void BALLThread::sendMessage_(Message* msg)
		{
			if (main_control_ == 0) return;
			// Qt will delete the MessageEvent when done
			qApp->postEvent(main_control_, new MessageEvent(msg));
		}

		// ================================== FetchHTMLThread ===============================

		FetchHTMLThread::FetchHTMLThread()
			throw()
			: BALLThread(),
				file_name_("")
		{
		}

		void FetchHTMLThread::setURL(const String& url)
			throw()
		{
			url_ = url;
		}

		void FetchHTMLThread::run()
		{
			if (main_control_ == 0) return;

			if (url_ == "")
			{
				output_("Invalid Address " + url_ + " in " + String(__FILE__) + ":" + String(__LINE__), true);
				return;
			}

			if (main_control_ != 0)
			{
				tcp_.setProxy(main_control_->getProxy(), main_control_->getProxyPort());
			}

			try
			{
				if (file_name_ != "")
				{
					File f(file_name_, std::ios::out);
					if (!f.isOpen())
					{
						output_(String("Could not open file ") + file_name_ + " for output!");
						return;
					}
					tcp_.set(f, url_);
					// dont move the transfer() call away from here!
					tcp_.transfer();
				}
				else
				{
					char c;
					stream_.get(c);

					while (stream_.gcount() > 0)
					{
						stream_.get(c);
					}

					stream_.clear();

					tcp_.set(stream_, url_);
					// dont move the transfer() call away from here!
					tcp_.transfer();
				}
			}
			catch(...)
			{
				output_(String("Exception in ") + String(__FILE__) + ": " + String(__LINE__), true);
			}
		}

		// ==========================================

		UpdateRepresentationThread::UpdateRepresentationThread()
			throw()
			: BALLThread(),
				rep_(0)
		{
			setTerminationEnabled(true);
		}

		void UpdateRepresentationThread::run()
		{
			if (main_control_ == 0) return;

			RepresentationManager& pm = main_control_->getRepresentationManager();
			Representation* rep = 0;
			while (!main_control_->isAboutToQuit())
			{
				if (!main_control_->useMultithreading())
				{
					msleep(50);
					continue;
				}

				rep = pm.popRepresentationToUpdate();
				if (rep == 0)
				{
					msleep(10);
					continue;
				}

				rep->update_();

				sendMessage_(new RepresentationMessage(*rep, RepresentationMessage::FINISHED_UPDATE));
			}
		}

		// ==========================================

		SimulationThread::SimulationThread()
			: BALLThread(),
				steps_between_updates_(0),
				dcd_file_(0)
		{
			setTerminationEnabled(true);
		}

		void SimulationThread::exportSceneToPNG_()
		{
			if (main_control_->stopedSimulation()) return;

			sendMessage_(new SceneMessage(SceneMessage::EXPORT_PNG));
		}

		void SimulationThread::finish_()
		{
			if (dcd_file_ != 0)
			{
				dcd_file_->close();
				String filename = dcd_file_->getName();
				delete dcd_file_;
				dcd_file_ = 0;
				// we will reopen the file to prevent problems when a user runs an other sim
				SnapShotManagerDataset* set = new SnapShotManagerDataset;
				set->setName(filename);
				set->setType(TrajectoryController::type);
				set->setComposite(getComposite());
				SnapShotManager* manager = new SnapShotManager((System*)getComposite(), 0,
																					new DCDFile(filename, std::ios::in));
				set->setData(manager);
				sendMessage_(new DatasetMessage(set, DatasetMessage::ADD));
			}
			sendMessage_(new FinishedSimulationMessage);
		}

		// =====================================================================
		void EnergyMinimizerThread::run()
		{
			if (main_control_ == 0) return;

			try
			{
				if (minimizer_ == 0 															||
						minimizer_->getForceField() == 0 							||
						minimizer_->getForceField()->getSystem() == 0 ||
						main_control_ == 0)
				{
					throw Exception::NullPointer(__FILE__, __LINE__);
				}

				ForceField& ff = *minimizer_->getForceField();
				bool ok = true;
				bool converged = false;
				// iterate until done and refresh the screen every "steps" iterations
				while (!main_control_->stopedSimulation() &&
								minimizer_->getNumberOfIterations() < minimizer_->getMaxNumberOfIterations() &&
								!converged && ok)
				{
					converged = minimizer_->minimize(steps_between_updates_, true);
					ok = !minimizer_->wasAborted();

					updateStructure_();
					waitForUpdateOfRepresentations_();

					QString message;
					message.sprintf("Iteration %d: energy = %f kJ/mol, RMS gradient = %f kJ/mol A",
													minimizer_->getNumberOfIterations(),
													ff.getEnergy(), ff.getRMSGradient());
					output_(ascii(message));
				}

				updateStructure_();

				output_(ff.getResults());
				output_("final RMS gradient    : " + String(ff.getRMSGradient()) + " kJ/(mol A)   after "
								+ String(minimizer_->getNumberOfIterations()) + " iterations\n",
								true);

				if (converged) output_("converged!");
				if (!ok) output_("aborted!");
				if (minimizer_->getNumberOfIterations() == minimizer_->getMaxNumberOfIterations()) output_("max number of iterations reached!");

				finish_();

				if (!ok)
				{
					output_("Aborted minimization because convergence could not be reached. Try to restart the minimization.", true);
					return;
				}
			}
			catch(Exception::GeneralException e)
			{
				delete dcd_file_;
				dcd_file_ = 0;

				String txt = String("Exception was thrown during minimization: ") + __FILE__ + ": " + String(__LINE__) + " :\n"
											+ e.getMessage();
				output_(txt, true);

				finish_();
			}
		}

		EnergyMinimizerThread::EnergyMinimizerThread()
			: SimulationThread(),
				minimizer_(0)
		{
		}

		EnergyMinimizerThread::~EnergyMinimizerThread()
		{
			if (minimizer_ != 0)
			{
				delete minimizer_;
			}
		}

		void EnergyMinimizerThread::setEnergyMinimizer(EnergyMinimizer* minimizer)
		{
			if (minimizer_ != 0)
			{
				delete minimizer_;
			}
			minimizer_ = minimizer;
		}

		// =====================================================
		void MDSimulationThread::run()
			throw(Exception::NullPointer)
		{
			if (main_control_ == 0) return;

			try
			{
				if (md_ == 0 ||
						md_->getForceField() == 0 ||
						md_->getForceField()->getSystem() == 0 ||
						main_control_ == 0)
				{
					throw Exception::NullPointer(__FILE__, __LINE__);
				}
				ForceField& ff = *md_->getForceField();

				SnapShotManager manager(ff.getSystem(), &ff, dcd_file_);
				manager.setFlushToDiskFrequency(10);
				bool ok = true;

				// iterate until done and refresh the screen every "steps" iterations
				while (ok &&
							 md_->getNumberOfIterations() < steps_ &&
							 !main_control_->stopedSimulation())
				{
					ok = md_->simulateIterations(steps_between_updates_, true);
					updateStructure_();

					waitForUpdateOfRepresentations_();

					QString message;
					message.sprintf("Iteration %d: energy = %f kJ/mol, RMS gradient = %f kJ/mol A",
													md_->getNumberOfIterations(), ff.getEnergy(),
													ff.getRMSGradient());
					output_(ascii(message));

					if (save_images_) exportSceneToPNG_();
					if (dcd_file_) 		manager.takeSnapShot();
				}

				if (dcd_file_) manager.flushToDisk();

 				output_(ff.getResults());
				output_("final RMS gradient    : " + String(ff.getRMSGradient()) + " kJ/(mol A)   after "
								+ String(md_->getNumberOfIterations()) + " iterations\n",
								true);

				if (!ok)
				{
					output_("Simulation aborted to to strange energy values.", true);
				}

				finish_();
			}
			catch(Exception::GeneralException e)
			{
				String txt = String("Exception was thrown during MD simulation: ")
											+ __FILE__ + ": " + String(__LINE__) + " \n" + e.getMessage();
				output_(txt, true);

				if (dcd_file_ != 0)
				{
					dcd_file_->close();
					delete dcd_file_;
					dcd_file_ = 0;
				}

				finish_();
			}
		}

		MDSimulationThread::MDSimulationThread()
			: SimulationThread(),
				md_(0),
				save_images_(false)
		{
		}

		MDSimulationThread::~MDSimulationThread()
		{
			if (md_ != 0) delete md_;
		}

		void MDSimulationThread::setMolecularDynamics(MolecularDynamics* md)
		{
			if (md_ != 0) delete md_;

			md_ = md;
		}

		// =================================================0

		CalculateFDPBThread::CalculateFDPBThread()
			throw()
			: BALLThread(),
				dialog_(0)
		{}

		void CalculateFDPBThread::run()
		{
			if (dialog_ == 0) return;
			dialog_->calculate_();
		}

		// =========================== implementation of class DockingThread ================

		///
		DockingThread::DockingThread()
			throw()
			: BALLThread(),
				dock_alg_(0)
		{}

		// Copy constructor.
		DockingThread::DockingThread(const DockingThread& dock_thread)
			throw()
			: BALLThread(),
				dock_alg_(dock_thread.dock_alg_)
		{}

		///
		DockingThread::~DockingThread()
			throw()
		{
			output_("delete thread", true);

			// docking algorithm is deleted in DockingController
		}

		// Assignment operator
		const DockingThread& DockingThread::operator =(const DockingThread& dock_thread)
			throw()
		{
			if (&dock_thread != this)
			{
				dock_alg_ = dock_thread.dock_alg_;
			}
			return *this;
		}

		//
		void DockingThread::setDockingAlgorithm(DockingAlgorithm* dock_alg)
			throw()
		{
			dock_alg_ = dock_alg;
		}

		///
		void DockingThread::run()
			throw(Exception::NullPointer)
		{
				if (dock_alg_ == 0 ||
						main_control_ == 0)
				{
					throw Exception::NullPointer(__FILE__, __LINE__);
				}

				output_("starting docking...", true);

				dock_alg_->start();

				DockingFinishedMessage* msg = new DockingFinishedMessage(dock_alg_->wasAborted());
				// conformation set is deleted in DockResult
				msg->setConformationSet(new ConformationSet(dock_alg_->getConformationSet()));
				sendMessage_(msg);

				output_("Docking finished.", true);
		}

		// =================================================0

	} // namespace VIEW
} // namespace BALL

<|endoftext|>