text:
/*
 *  Copyright (c) 2013, PAL Robotics, S.L.
 *  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 PAL Robotics 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: Bence Magyar
 */

#include <cmath>

#include <boost/assign.hpp>

#include <Eigen/Dense>

#include <tf/transform_datatypes.h>
#include <urdf_parser/urdf_parser.h>
#include <urdf/urdfdom_compatibility.h>

#include <talon_swerve_drive_controller/swerve_drive_controller.h>

//TODO: include swerve stuff from C-Control
using Eigen::Vector2d;
using std::array;
using Eigen::Affine2d;
using Eigen::Matrix2d;
using Eigen::Vector2d;

using ros::Time;
using geometry_msgs::TwistConstPtr;
using ros::Duration;

const std::string talon_swerve_drive_controller::TalonSwerveDriveController::DEF_BASE_LINK = "base_link";
const double talon_swerve_drive_controller::TalonSwerveDriveController::DEF_ODOM_PUB_FREQ = 50.;
const bool talon_swerve_drive_controller::TalonSwerveDriveController::DEF_PUB_ODOM_TO_BASE = false;
const std::string talon_swerve_drive_controller::TalonSwerveDriveController::DEF_ODOM_FRAME = "odom";
const std::string talon_swerve_drive_controller::TalonSwerveDriveController::DEF_BASE_FRAME = "base_link";
const double talon_swerve_drive_controller::TalonSwerveDriveController::DEF_INIT_X = 0.;
const double talon_swerve_drive_controller::TalonSwerveDriveController::DEF_INIT_Y = 0.;
const double talon_swerve_drive_controller::TalonSwerveDriveController::DEF_INIT_YAW = 0.;
const double talon_swerve_drive_controller::TalonSwerveDriveController::DEF_SD = 0.01;

/*
static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2)
{
  return std::sqrt(std::pow(vec1.x-vec2.x,2) +
                   std::pow(vec1.y-vec2.y,2) +
                   std::pow(vec1.z-vec2.z,2));
}
*/
/*
* \brief Check that a link exists and has a geometry collision.
* \param link The link
* \return true if the link has a collision element with geometry
*/
static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr &link)
{
	if (!link)
	{
		ROS_ERROR("Link == NULL.");
		return false;
	}

	if (!link->collision)
	{
		ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
		return false;
	}

	if (!link->collision->geometry)
	{
		ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
		return false;
	}
	return true;
}
#if 0
/*
 * \brief Check if the link is modeled as a cylinder
 * \param link Link
 * \return true if the link is modeled as a Cylinder; false otherwise
 */
static bool isCylinder(const urdf::LinkConstSharedPtr &link)
{
	if (!hasCollisionGeometry(link))
	{
		return false;
	}

	if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
	{
		ROS_DEBUG_STREAM("Link " << link->name << " does not have cylinder geometry");
		return false;
	}

	return true;
}

/*
 * \brief Check if the link is modeled as a sphere
 * \param link Link
 * \return true if the link is modeled as a Sphere; false otherwise
 *
 * \param link Link
 * \return true if the link is modeled as a Sphere; false otherwise
 */
static bool isSphere(const urdf::LinkConstSharedPtr &link)
{
	if (!hasCollisionGeometry(link))
	{
		return false;
	}

	if (link->collision->geometry->type != urdf::Geometry::SPHERE)
	{
		ROS_DEBUG_STREAM("Link " << link->name << " does not have sphere geometry");
		return false;
	}

	return true;
}
#endif

/*
 * \brief Get the wheel radius
 * \param [in]  wheel_link   Wheel link
 * \param [out] wheel_radius Wheel radius [m]
 * \return true if the wheel radius was found; false otherwise
 */
#if 0
static bool getWheelRadius(const urdf::LinkConstSharedPtr &wheel_link, double &wheel_radius)
{
	if (isCylinder(wheel_link))
	{
		wheel_radius = (static_cast<urdf::Cylinder *>(wheel_link->collision->geometry.get()))->radius;
		return true;
	}
	else if (isSphere(wheel_link))
	{
		wheel_radius = (static_cast<urdf::Sphere *>(wheel_link->collision->geometry.get()))->radius;
		return true;
	}

	ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder or sphere!");
	return false;
}
#endif

namespace talon_swerve_drive_controller
{

TalonSwerveDriveController::TalonSwerveDriveController() :
	open_loop_(false),
	wheel_radius_(0.0),
	cmd_vel_timeout_(0.5), //Change to 5.0 for auto path planning testing
	allow_multiple_cmd_vel_publishers_(true),
	base_frame_id_("base_link"),
	odom_frame_id_("odom"),
	enable_odom_tf_(true),
	wheel_joints_size_(0),
	publish_cmd_(false)

	//model_({0, 0, 0, 0, 0, 0}),
	//invertWheelAngle_(false),
	//units_({1,1,1,1,1,1}),
	//driveRatios_({0, 0, 0}),
	//units_({0, 0, 0, 0})
{
}

bool TalonSwerveDriveController::init(hardware_interface::TalonCommandInterface *hw,
									  ros::NodeHandle &/*root_nh*/,
									  ros::NodeHandle &controller_nh)
{
	const std::string complete_ns = controller_nh.getNamespace();
	std::size_t id = complete_ns.find_last_of("/");
	name_ = complete_ns.substr(id + 1);

	mode_.writeFromNonRT(true);

	// Get joint names from the parameter server
	std::vector<std::string> speed_names, steering_names;
	if (!getWheelNames(controller_nh, "speed", speed_names) or
			!getWheelNames(controller_nh, "steering", steering_names))
	{
		return false;
	}

	if (speed_names.size() != steering_names.size())
	{
		ROS_ERROR_STREAM_NAMED(name_,
							   "#speed (" << speed_names.size() << ") != " <<
							   "#steering (" << steering_names.size() << ").");
		return false;
	}
	else
	{
		wheel_joints_size_ = speed_names.size();

		speed_joints_.resize(wheel_joints_size_);
		steering_joints_.resize(wheel_joints_size_);
	}

	/*ros::NodeHandle n; //Is this bad?

    ros::NodeHandle n_params_behaviors(n, "auto_params");

	if (!n_params_behaviors.getParam("num_profile_slots", num_profile_slots_))
            ROS_ERROR("Didn't read param num_profile_slots in talon_swerve");
	*/num_profile_slots_ = 20;

	// Odometry related:
	double publish_rate;
	std::string base_link;
        controller_nh.param("base_link", base_link, DEF_BASE_LINK);
	controller_nh.param("publish_rate", publish_rate, 50.0);
	ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
						  << publish_rate << "Hz.");
	publish_period_ = ros::Duration(1.0 / publish_rate);

	// Publish limited velocity:
	//controller_nh.param("publish_cmd", publish_cmd_, publish_cmd_);

	// TODO : see if model_, driveRatios, units can be local instead of member vars
	// If either parameter is not available, we need to look up the value in the URDF
	//bool lookup_wheel_coordinates = !controller_nh.getParam("wheel_coordinates", wheel_coordinates_);
	bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
	bool lookup_max_speed = !controller_nh.getParam("max_speed", model_.maxSpeed);
	bool lookup_mass = !controller_nh.getParam("mass", model_.mass);
	bool lookup_motor_free_speed = !controller_nh.getParam("motor_free_speed", model_.motorFreeSpeed);
	bool lookup_motor_stall_torque = !controller_nh.getParam("motor_stall_torque", model_.motorStallTorque);
	// TODO : why not just use the number of wheels read from yaml?
	bool lookup_motor_quantity = !controller_nh.getParam("motor_quantity", model_.motorQuantity);
	bool lookup_invert_wheel_angle = !controller_nh.getParam("invert_wheel_angle", invertWheelAngle_);
	bool lookup_ratio_encoder_to_rotations = !controller_nh.getParam("ratio_encoder_to_rotations", driveRatios_.encodertoRotations);
	bool lookup_ratio_motor_to_rotations = !controller_nh.getParam("ratio_motor_to_rotations", driveRatios_.motortoRotations);
	bool lookup_ratio_motor_to_steering = !controller_nh.getParam("ratio_motor_to_steering", driveRatios_.motortoSteering); // TODO : not used?
	bool lookup_encoder_drive_get_V_units = !controller_nh.getParam("encoder_drive_get_V_units", units_.rotationGetV);
	bool lookup_encoder_drive_get_P_units = !controller_nh.getParam("encoder_drive_get_P_units", units_.rotationGetP);
	bool lookup_encoder_drive_set_V_units = !controller_nh.getParam("encoder_drive_set_V_units", units_.rotationSetV);
	bool lookup_encoder_drive_set_P_units = !controller_nh.getParam("encoder_drive_set_P_units", units_.rotationSetP);
	bool lookup_encoder_steering_get_units = !controller_nh.getParam("encoder_steering_get_units", units_.steeringGet);
	bool lookup_encoder_steering_set_units = !controller_nh.getParam("encoder_steering_set_units", units_.steeringSet);
	bool lookup_f_static = !controller_nh.getParam("f_static", f_static_); //TODO: Maybe use this?
	std::vector<double> wheel1a;
	std::vector<double> wheel2a;
	std::vector<double> wheel3a;
	std::vector<double> wheel4a;
	bool lookup_wheel1x = !controller_nh.getParam("wheel_coords1x", wheel_coords_[0][0]);
	bool lookup_wheel2x = !controller_nh.getParam("wheel_coords2x", wheel_coords_[1][0]);
	bool lookup_wheel3x = !controller_nh.getParam("wheel_coords3x", wheel_coords_[2][0]);
	bool lookup_wheel4x = !controller_nh.getParam("wheel_coords4x", wheel_coords_[3][0]);
	bool lookup_wheel1y = !controller_nh.getParam("wheel_coords1y", wheel_coords_[0][1]);
	bool lookup_wheel2y = !controller_nh.getParam("wheel_coords2y", wheel_coords_[1][1]);
	bool lookup_wheel3y = !controller_nh.getParam("wheel_coords3y", wheel_coords_[2][1]);
	bool lookup_wheel4y = !controller_nh.getParam("wheel_coords4y", wheel_coords_[3][1]);

	ROS_INFO_STREAM("Coords: " << wheel_coords_[0] << "   "<< wheel_coords_[1] << "   "<< wheel_coords_[2] << "   "<< wheel_coords_[3]);
	std::vector<double> offsets;
	for (auto it = steering_names.cbegin(); it != steering_names.cend(); ++it)
	{
		ros::NodeHandle nh(controller_nh, *it);
		double dbl_val = 0;
		if (!nh.getParam("offset", dbl_val))
			ROS_ERROR_STREAM("Can not read offset for " << *it);
		offsets.push_back(dbl_val);
	}

	profile_queue_num = controller_nh.advertise<std_msgs::UInt16>("profile_queue_num", 1);

	/*
	if (!setOdomParamsFromUrdf(root_nh,
	                          speed_names[0],
	                          steering_names[0],
	                          //lookup_wheel_coordinates,
	                          lookup_wheel_radius))
	{
	  return false;
	}

	// Regardless of how we got the separation and radius, use them
	*/
	// to set the odometry parameters
	//setOdomPubFields(root_nh, controller_nh);

	/*if (publish_cmd_)
	{
	  cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped>(controller_nh, "cmd_vel_out", 100));
	}
	*/
	// Get the joint object to use in the realtime loop

	// TODO : all of these need to be read from params
	/*
	model.maxSpeed = 3.3528;
	model.mass = 70;
	model.motorFreeSpeed = 5330;
	model.motorStallTorque = 2.41;
	model.motorQuantity = 4;
	*/
	model_.wheelRadius =  wheel_radius_;

	/*
	invertWheelAngle(false);
	swerveVar::ratios driveRatios({20, 7, 7});
	swerveVar::encoderUnits units({1,1,1,1,1,1});
	*/

	swerveC_ = std::make_shared<swerve>(wheel_coords_, offsets, invertWheelAngle_, driveRatios_, units_, model_);
	for (size_t i = 0; i < wheel_joints_size_; ++i)
	{
		ROS_INFO_STREAM_NAMED(name_,
							  "Adding speed motors with joint name: " << speed_names[i]
							  << " and steering motors with joint name: " << steering_names[i]);

		ros::NodeHandle l_nh(controller_nh, speed_names[i]);
		speed_joints_[i].initWithNode(hw, nullptr, l_nh);
		ros::NodeHandle r_nh(controller_nh, steering_names[i]);
		steering_joints_[i].initWithNode(hw, nullptr, r_nh);
	}

	sub_command_ = controller_nh.subscribe("cmd_vel", 1, &TalonSwerveDriveController::cmdVelCallback, this);
	brake_serv_ = controller_nh.advertiseService("brake", &TalonSwerveDriveController::brakeService, this);
	motion_profile_serv_ = controller_nh.advertiseService("run_profile", &TalonSwerveDriveController::motionProfileService, this);
	wheel_pos_serv_ = controller_nh.advertiseService("wheel_pos", &TalonSwerveDriveController::wheelPosService, this);
	//sub_run_profile_ = controller_nh.subscribe("run_profile", 1, &TalonSwerveDriveController::runCallback, this);

	double odom_pub_freq;
        controller_nh.param("odometry_publishing_frequency", odom_pub_freq, DEF_ODOM_PUB_FREQ);

	comp_odom_ = odom_pub_freq > 0;
	//ROS_WARN("COMPUTING ODOM");
	if (comp_odom_)
	{
		odom_pub_period_ = Duration(1 / odom_pub_freq);
		controller_nh.param("publish_odometry_to_base_transform", pub_odom_to_base_,
				DEF_PUB_ODOM_TO_BASE);

		double init_x, init_y, init_yaw;
		controller_nh.param("initial_x", init_x, DEF_INIT_X);
		controller_nh.param("initial_y", init_y, DEF_INIT_Y);
		controller_nh.param("initial_yaw", init_yaw, DEF_INIT_YAW);
		double x_sd, y_sd, yaw_sd;
		controller_nh.param("x_sd", x_sd, DEF_SD);
		controller_nh.param("y_sd", y_sd, DEF_SD);
		controller_nh.param("yaw_sd", yaw_sd, DEF_SD);
		double x_speed_sd, y_speed_sd, yaw_speed_sd;
		controller_nh.param("x_speed_sd", x_speed_sd, DEF_SD);
		controller_nh.param("y_speed_sd", y_speed_sd, DEF_SD);
		controller_nh.param("yaw_speed_sd", yaw_speed_sd, DEF_SD);

		init_odom_to_base_.setIdentity();
		init_odom_to_base_.rotate(init_yaw);
		init_odom_to_base_.translation() = Vector2d(init_x, init_y);
		odom_to_base_ = init_odom_to_base_;
		odom_rigid_transf_.setIdentity();

		wheel_pos_.resize(2, WHEELCOUNT);
		//ROS_WARN("working h");
		for(size_t i = 0; i < WHEELCOUNT; i++)
		{
			//ROS_INFO_STREAM("id: " << i << "pos" << wheel_coords_[i]);
			wheel_pos_.col(i) = wheel_coords_[i];
			//ROS_WARN("f1.test");
		}

		const Vector2d centroid = wheel_pos_.rowwise().mean();
		wheel_pos_.colwise() -= centroid;
		neg_wheel_centroid_ = -centroid;

		new_wheel_pos_.resize(WHEELCOUNT, 2);

		std::string odom_frame, base_frame;
		controller_nh.param("odometry_frame", odom_frame, DEF_ODOM_FRAME);
		controller_nh.param("base_frame", base_frame, DEF_BASE_FRAME);

		odom_pub_.msg_.header.frame_id = odom_frame;
		odom_pub_.msg_.child_frame_id = base_frame;

		odom_pub_.msg_.pose.pose.position.z = 0;

		odom_pub_.msg_.pose.covariance.assign(0);
		odom_pub_.msg_.pose.covariance[0] = x_sd * x_sd;
		odom_pub_.msg_.pose.covariance[7] = y_sd * y_sd;
		odom_pub_.msg_.pose.covariance[35] = yaw_sd * yaw_sd;

		odom_pub_.msg_.twist.twist.linear.z = 0;
		odom_pub_.msg_.twist.twist.angular.x = 0;
		odom_pub_.msg_.twist.twist.angular.y = 0;

		odom_pub_.msg_.twist.covariance.assign(0);
		odom_pub_.msg_.twist.covariance[0] = x_speed_sd * x_speed_sd;
		odom_pub_.msg_.twist.covariance[7] = y_speed_sd * y_speed_sd;
		odom_pub_.msg_.twist.covariance[35] = yaw_speed_sd * yaw_speed_sd;
		odom_pub_.init(controller_nh, "odom", 1);

		if (pub_odom_to_base_)
		{
			odom_tf_pub_.msg_.transforms.resize(1);
			geometry_msgs::TransformStamped& odom_tf_trans =
				odom_tf_pub_.msg_.transforms[0];
			odom_tf_trans.header.frame_id = odom_pub_.msg_.header.frame_id;
			odom_tf_trans.child_frame_id = odom_pub_.msg_.child_frame_id;
			odom_tf_trans.transform.translation.z = 0;
			odom_tf_pub_.init(controller_nh, "/tf", 1);
		}

		for (size_t row = 0; row < WHEELCOUNT; row++)
		{
			old_wheel_pos_[row] = {0, 0};
			last_wheel_rot_[row] = speed_joints_[row].getPosition();
		}
	}

	return true;
}

void TalonSwerveDriveController::compOdometry(const Time& time, const double inv_delta_t)
{
	//ROS_INFO_STREAM("WORKS");
	// Compute the rigid transform from wheel_pos_ to new_wheel_pos_.

	std::array<double, WHEELCOUNT> steer_angles;
	for (size_t k = 0; k < WHEELCOUNT; k++)
	{
		const double new_wheel_rot = speed_joints_[k].getPosition();
		const double delta_rot = new_wheel_rot - last_wheel_rot_[k];
		//int inverterD = (k%2==0) ? -1 : 1;
		const double dist = -delta_rot * wheel_radius_ * driveRatios_.encodertoRotations; //* inverterD;
		//NOTE: below is a hack, TODO: REMOVE

		steer_angles[k] = steering_joints_[k].getPosition();
		const double steer_angle = swerveC_->getWheelAngle(k, steer_angles[k]);
		const Eigen::Vector2d delta_pos = {-dist*sin(steer_angle), dist*cos(steer_angle)};
		new_wheel_pos_(k, 0) = wheel_coords_[k][0] + delta_pos[0];
		new_wheel_pos_(k, 1) = wheel_coords_[k][1] + delta_pos[1];

		//ROS_INFO_STREAM("id: " << k << " delta: " << delta_pos << " steer: " << steer_angle << " dist: " << dist);
		last_wheel_rot_[k] = new_wheel_rot;
	}
	{
		std::lock_guard<std::mutex> lock(steer_angles_mutex_);
		steer_angles_ = steer_angles;
	}
	const Eigen::RowVector2d new_wheel_centroid =
		new_wheel_pos_.colwise().mean();
	new_wheel_pos_.rowwise() -= new_wheel_centroid;

	//ROS_INFO_STREAM("rows: " << wheel_pos_.rows() << " cols: " << wheel_pos_.cols());
	//ROS_INFO_STREAM("neg wheel centroid" << neg_wheel_centroid_ << " new centroid: " << new_wheel_centroid);

	const Matrix2d h = wheel_pos_ * new_wheel_pos_;
	const Eigen::JacobiSVD<Matrix2d> svd(h, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Matrix2d rot = svd.matrixV() * svd.matrixU().transpose();
	if (rot.determinant() < 0)
		rot.col(1) *= -1;

	odom_rigid_transf_.matrix().block(0, 0, 2, 2) = rot;
	odom_rigid_transf_.translation() =
		rot * neg_wheel_centroid_ + new_wheel_centroid.transpose();
	odom_to_base_ = odom_to_base_ * odom_rigid_transf_;

	const double odom_x = odom_to_base_.translation().x();
	const double odom_y = odom_to_base_.translation().y();
	const double odom_yaw = atan2(odom_to_base_(1, 0), odom_to_base_(0, 0));

	//ROS_INFO_STREAM("odom_x: " << odom_x << " odom_y: " << odom_y << " odom_yaw: " << odom_yaw);
	// Publish the odometry.
	//TODO CHECK THIS PUB

	geometry_msgs::Quaternion orientation;
	bool orientation_comped = false;

	// tf
	if (pub_odom_to_base_ && time - last_odom_tf_pub_time_ >= odom_pub_period_ &&
			odom_tf_pub_.trylock())
	{
		orientation = tf::createQuaternionMsgFromYaw(odom_yaw);
		orientation_comped = true;

		geometry_msgs::TransformStamped& odom_tf_trans =
			odom_tf_pub_.msg_.transforms[0];
		odom_tf_trans.header.stamp = time;
		odom_tf_trans.transform.translation.x = odom_x;
		odom_tf_trans.transform.translation.y = odom_y;
		odom_tf_trans.transform.rotation = orientation;
		ROS_INFO_STREAM(odom_x);
		odom_tf_pub_.unlockAndPublish();
		last_odom_tf_pub_time_ = time;
	}
	// odom
	if (time - last_odom_pub_time_ >= odom_pub_period_ && odom_pub_.trylock())
	{
		if (!orientation_comped)
			orientation = tf::createQuaternionMsgFromYaw(odom_yaw);

		odom_pub_.msg_.header.stamp = time;
		odom_pub_.msg_.pose.pose.position.x = odom_x;
		odom_pub_.msg_.pose.pose.position.y = odom_y;
		odom_pub_.msg_.pose.pose.orientation = orientation;

		odom_pub_.msg_.twist.twist.linear.x =
			odom_rigid_transf_.translation().x() * inv_delta_t;
		odom_pub_.msg_.twist.twist.linear.y =
			odom_rigid_transf_.translation().y() * inv_delta_t;
		odom_pub_.msg_.twist.twist.angular.z =
			atan2(odom_rigid_transf_(1, 0), odom_rigid_transf_(0, 0)) * inv_delta_t;

		odom_pub_.unlockAndPublish();
		last_odom_pub_time_ = time;
	}
}

void TalonSwerveDriveController::update(const ros::Time &time, const ros::Duration &period)
{
	const double delta_t = period.toSec();
	const double inv_delta_t = 1 / delta_t;
	if (comp_odom_) compOdometry(time, inv_delta_t);

	/*
	// COMPUTE AND PUBLISH ODOMETRY
	if (open_loop_)
	{
	  odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
	}
	else
	{
	  double left_pos  = 0.0;
	  double right_pos = 0.0;
	  for (size_t i = 0; i < wheel_joints_size_; ++i)
	  {
	    const double lp = speed_joints_[i].getPosition();
	    const double rp = steering_joints_[i].getPosition();
	    if (std::isnan(lp) || std::isnan(rp))
	      return;

	    left_pos  += lp;
	    right_pos += rp;
	  }
	  left_pos  /= wheel_joints_size_;
	  right_pos /= wheel_joints_size_;

	  // Estimate linear and angular velocity using joint information
	  odometry_.update(left_pos, right_pos, time);
	}

	// Publish odometry message
	if (last_state_publish_time_ + publish_period_ < time)
	{
	  last_state_publish_time_ += publish_period_;
	  // Compute and store orientation info
	  const geometry_msgs::Quaternion orientation(
	        tf::createQuaternionMsgFromYaw(odometry_.getHeading()));

	  // Populate odom message and publish
	  if (odom_pub_->trylock())
	  {
	    odom_pub_->msg_.header.stamp = time;
	    odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
	    odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
	    odom_pub_->msg_.pose.pose.orientation = orientation;
	    odom_pub_->msg_.twist.twist.linear.x  = odometry_.getLinear();
	    odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
	    odom_pub_->unlockAndPublish();
	  }

	  // Publish tf /odom frame
	  if (enable_odom_tf_ && tf_odom_pub_->trylock())
	  {
	    geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
	    odom_frame.header.stamp = time;
	    odom_frame.transform.translation.x = odometry_.getX();
	    odom_frame.transform.translation.y = odometry_.getY();
	    odom_frame.transform.rotation = orientation;
	    tf_odom_pub_->unlockAndPublish();
	  }
	}
	*/
	// MOVE ROBOT
	// Retreive current velocity command and time step:

	//ROS_INFO_STREAM("mode: " << *(mode_.readFromRT()));

	//For this to be thread safe, the assumption is that the serv is called relatively infrequently
	if(full_profile_buffer_.size() != 0)
	{
		//WHERE BE THIS MUTEX
		full_profile_cmd cur_prof_cmd = full_profile_buffer_.front();
		full_profile_buffer_.pop_front();
		if(cur_prof_cmd.brake)
		{
			ROS_WARN("profile_reset");
			//required for reset
			for(size_t k = 0; k < WHEELCOUNT; k++)
			{
				steering_joints_[k].setCustomProfileRun(false);
				speed_joints_[k].setCustomProfileRun(false);
			}
			brake_struct_other_.lin[0] = 0;
			brake_struct_other_.lin[1] = 0;
			brake_struct_other_.ang = 0;
			brake_struct_other_.stamp = ros::Time::now();
			ROS_WARN("called in controller");
			command_.writeFromNonRT(brake_struct_other_);
			mode_.writeFromNonRT (true);
		}

		if(cur_prof_cmd.wipe_all)
		{
			ROS_WARN("profile_wipe");
			for(int i = 0; i < num_profile_slots_; i++)
			{
				for(size_t k = 0; k < WHEELCOUNT; k++)
				{
					full_profile_[k][0].clear();
					full_profile_[k][1].clear();
					speed_joints_[k].overwriteCustomProfilePoints(full_profile_[k][0], i);
					steering_joints_[k].overwriteCustomProfilePoints(full_profile_[k][1], i);
				}
			}
		}

		if(cur_prof_cmd.buffer)
		{
			ROS_WARN("buffer in controller - pre loop");
			for(size_t p = 0; p < cur_prof_cmd.profiles.size(); p++)
			{
				ROS_WARN("buffer in controller");
				const int point_count2 = cur_prof_cmd.profiles[p].drive_pos.size();
				ROS_INFO_STREAM("points: " << point_count2);
				for(size_t i = 0; i < WHEELCOUNT; i++)
				{
					holder_points_[i][0].mode = cur_prof_cmd.profiles[p].hold[0][i] ? hardware_interface::TalonMode_PercentOutput : hardware_interface::TalonMode_Position;
					holder_points_[i][1].mode = cur_prof_cmd.profiles[p].hold[0][i] ? hardware_interface::TalonMode_MotionMagic : hardware_interface::TalonMode_Position;

					holder_points_[i][0].pidSlot = 1;
					holder_points_[i][1].pidSlot = cur_prof_cmd.profiles[p].hold[0][i] ? 0 : 1; //0 and 1 are the same right now

					holder_points_[i][0].setpoint =  cur_prof_cmd.profiles[p].hold[0][i] ? 0 : cur_prof_cmd.profiles[p].drive_pos[0][i];
					holder_points_[i][1].setpoint = cur_prof_cmd.profiles[p].steer_pos[0][i];

					holder_points_[i][0].fTerm = cur_prof_cmd.profiles[p].hold[0][i] ? 0 : cur_prof_cmd.profiles[p].drive_f[0][i];
					holder_points_[i][1].fTerm = cur_prof_cmd.profiles[p].hold[0][i] ? 0 : cur_prof_cmd.profiles[p].steer_f[0][i];

					holder_points_[i][0].duration = cur_prof_cmd.profiles[p].dt;
					holder_points_[i][1].duration = cur_prof_cmd.profiles[p].dt;

					holder_points_[i][0].zeroPos = true;
					holder_points_[i][1].zeroPos = false;

					full_profile_[i][0].clear();
					full_profile_[i][1].clear();

					full_profile_[i][0].push_back(holder_points_[i][0]); //Rather than buffering like this we should write directly to full profile at some point
					full_profile_[i][1].push_back(holder_points_[i][1]); //Rather than buffering like this we should write directly to full profile at some point

					holder_points_[i][0].zeroPos = false;
				}

				const int point_count = cur_prof_cmd.profiles[p].drive_pos.size();
				ROS_INFO_STREAM("points: " << point_count);
				for(int i = 1; i < point_count; i++)
				{
					for(size_t k = 0; k < WHEELCOUNT; k++)
					{
						holder_points_[k][0].mode = cur_prof_cmd.profiles[p].hold[i][k] ? hardware_interface::TalonMode_PercentOutput : hardware_interface::TalonMode_Position;
						holder_points_[k][1].mode = cur_prof_cmd.profiles[p].hold[i][k] ? hardware_interface::TalonMode_MotionMagic : hardware_interface::TalonMode_Position;

						holder_points_[k][0].setpoint = cur_prof_cmd.profiles[p].hold[i][k] ? 0 : cur_prof_cmd.profiles[p].drive_pos[i][k];
						holder_points_[k][1].setpoint = cur_prof_cmd.profiles[p].steer_pos[i][k];

						holder_points_[k][0].fTerm = cur_prof_cmd.profiles[p].hold[i][k] ? 0 : cur_prof_cmd.profiles[p].drive_f[i][k];
						holder_points_[k][1].fTerm = cur_prof_cmd.profiles[p].hold[i][k] ? 0 : cur_prof_cmd.profiles[p].steer_f[i][k];
						//ROS_INFO_STREAM("f: " << 	holder_points_[k][0].fTerm);

						holder_points_[k][1].pidSlot = cur_prof_cmd.profiles[p].hold[i][k] ? 0 : 1;

						full_profile_[k][0].push_back(holder_points_[k][0]); //Rather than buffering like this we should write directly to full profile at some point
						full_profile_[k][1].push_back(holder_points_[k][1]); //Rather than buffering like this we should write directly to full profile at some point
					}
				}
				ROS_WARN("done1");
				for(size_t k = 0; k < WHEELCOUNT; k++)
				{
					speed_joints_[k].overwriteCustomProfilePoints(full_profile_[k][0], cur_prof_cmd.profiles[p].slot);
					steering_joints_[k].overwriteCustomProfilePoints(full_profile_[k][1], cur_prof_cmd.profiles[p].slot);
				}

				ROS_WARN("done");
			}
		}

		if(cur_prof_cmd.run)
		{
			ROS_WARN("running from  controller");
			mode_.writeFromNonRT(false); //Should be fine
			for(size_t k = 0; k < WHEELCOUNT; k++)
			{
				steering_joints_[k].setCustomProfileSlot(cur_prof_cmd.run_slot);
				speed_joints_[k].setCustomProfileSlot(cur_prof_cmd.run_slot);
			}
		}

		if(cur_prof_cmd.change_queue)
		{
			for(size_t k = 0; k < WHEELCOUNT; k++)
			{
				steering_joints_[k].setCustomProfileNextSlot(cur_prof_cmd.new_queue);
				speed_joints_[k].setCustomProfileNextSlot(cur_prof_cmd.new_queue);
			}
		}

	}
	static double mode_last = ros::Time::now().toSec();
	if(*(mode_.readFromRT()))
	{

		Commands curr_cmd = *(command_.readFromRT());
		const double dt = (time - curr_cmd.stamp).toSec();

		//ROS_INFO_STREAM("ang_vel_tar: " << curr_cmd.ang << " lin_vel_tar: " << curr_cmd.lin);

		// Brake if cmd_vel has timeout:
		if (dt > cmd_vel_timeout_)
		{
			curr_cmd.lin = {0.0, 0.0};
			curr_cmd.ang = 0.0;
		}

		static std::array<Vector2d, WHEELCOUNT> speeds_angles;
		static double time_before_brake = 0;

		for (size_t i = 0; i < wheel_joints_size_; ++i)
		{
			steering_joints_[i].setCustomProfileRun(false);
			speed_joints_[i].setCustomProfileRun(false);

			steering_joints_[i].setPIDFSlot(0);
			speed_joints_[i].setPIDFSlot(0);
			steering_joints_[i].setMode(position_mode);
			speed_joints_[i].setClosedloopRamp(0);

			speed_joints_[i].setDemand1Value(0);
			steering_joints_[i].setDemand1Value(0);
		}
		static double brake_last = ros::Time::now().toSec();
		if (fabs(curr_cmd.lin[0]) <= 1e-6 && fabs(curr_cmd.lin[1]) <= 1e-6 && fabs(curr_cmd.ang) <= 1e-6)
		{
			brake_last = ros::Time::now().toSec();

			for (size_t i = 0; i < wheel_joints_size_; ++i)
			{
				//ROS_INFO_STREAM("id:" << i << " speed: " <<speeds_angles[i][0]);
				speed_joints_[i].setCommand(0);
				speed_joints_[i].setMode(percent_voltage_mode);
			}
			if(ros::Time::now().toSec() - time_before_brake > .5)
			{
				brake();
			}
			else
			{
				for (size_t i = 0; i < wheel_joints_size_; ++i)
				{
					steering_joints_[i].setCommand(speeds_angles[i][1]);
				}

			}
			return;
		}

		time_before_brake = ros::Time::now().toSec();

		// Limit velocities and accelerations:
		//const double cmd_dt(period.toSec());

		// Compute wheels velocities:
		//Parse curr_cmd to get velocity vector and rotation (z axis)
		//TODO: check unit conversions/coordinate frames

		array<double, WHEELCOUNT> curPos;
		for (int i = 0; i < WHEELCOUNT; i++)
			curPos[i] = steering_joints_[i].getPosition();
		std::array<bool, WHEELCOUNT> holder;
		speeds_angles  = swerveC_->motorOutputs(curr_cmd.lin, curr_cmd.ang, M_PI/2, false, holder, false, curPos, true);

		// Set wheels velocities:
		for (size_t i = 0; i < wheel_joints_size_; ++i)
		{
			//ROS_INFO_STREAM("id:" << i << " speed: " <<speeds_angles[i][0]);

			steering_joints_[i].setCommand(speeds_angles[i][1]);
		}

		if(ros::Time::now().toSec() - .1 > brake_last || ros::Time::now().toSec() - .1 > mode_last)
		{
			for (size_t i = 0; i < wheel_joints_size_; ++i)
			{
				speed_joints_[i].setMode(velocity_mode);
				speed_joints_[i].setCommand(speeds_angles[i][0]);
			}
		}
		else
		{
			for (size_t i = 0; i < wheel_joints_size_; ++i)
			{
				speed_joints_[i].setCommand(0);
				speed_joints_[i].setMode(percent_voltage_mode);
			}
		}
	}
	else
	{
		mode_last =::Time::now().toSec();
		for (size_t i = 0; i < wheel_joints_size_; ++i)
		{
			steering_joints_[i].setCustomProfileRun(true);
			speed_joints_[i].setCustomProfileRun(true);

			//ROS_ERROR_STREAM(slot_local);
		}

	}

	static uint16_t slot_ret = 0;
	static int slot_ret_diff_last_sum;

	for (size_t i = 0; i < wheel_joints_size_; ++i)
	{
		if(slot_ret != steering_joints_[i].getCustomProfileSlot()) slot_ret_diff_last_sum+=1;

		slot_ret = steering_joints_[i].getCustomProfileSlot();

		//ROS_ERROR_STREAM(slot_local);
	}

	std_msgs::UInt16 pub_queue_hold;
	pub_queue_hold.data = slot_ret;

	profile_queue_num.publish(pub_queue_hold);
	if(slot_ret_diff_last_sum > 20)
	{

		ROS_ERROR("potential profile slot issue with swerve");
	}
}

void TalonSwerveDriveController::starting(const ros::Time &time)
{
	brake();

	// Register starting time used to keep fixed rate
	if (comp_odom_)
	{
		last_odom_pub_time_ = time;
		last_odom_tf_pub_time_ = time;
	}
	//odometry_.init(time);
}

void TalonSwerveDriveController::stopping(const ros::Time & /*time*/)
{
	brake();
}

void TalonSwerveDriveController::brake()
{
	//required input, but not needed in this case
	array<bool, WHEELCOUNT> hold;
	//Use parking config

	array<double, WHEELCOUNT> curPos;
	for (int i = 0; i < WHEELCOUNT; i++)
	{
		curPos[i] = steering_joints_[i].getPosition();
	}
	std::array<Vector2d, WHEELCOUNT> park = swerveC_->motorOutputs({0, 0}, 0, 0, false, hold, true, curPos, false);
	for (size_t i = 0; i < wheel_joints_size_; ++i)
	{
		speed_joints_[i].setCommand(0.0);
		steering_joints_[i].setCommand(park[i][1]);
	}
}

void TalonSwerveDriveController::cmdVelCallback(const geometry_msgs::Twist &command)
{
	if (isRunning())
	{
		// check tha//t we don't have multiple publishers on the command topic
        //ROS_WARN("Time Difference: %f", ros::Time::now().toSec() - command->header.stamp.toSec());
		if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1)
		{
			ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers()
											<< " publishers. Only 1 publisher is allowed. Going to brake.");
			brake();
			return;
		}

		//These below are some simple bounds checks on the cmd vel input so we don't make dumb mistakes. (like try to get the swerve drive to fly away)
		//Those counters exist to reduce spam somewhat
		static int fly_counter = 0;
		static bool fly_last = false;
		if(command.linear.z != 0)
		{
			if(fly_counter > 40 || !fly_last)
			{
				ROS_ERROR("Rotors not up to speed!");
				fly_counter = 0;
			}
			fly_last = true;
			fly_counter++;
		}
		else
		{
			fly_last = false;
		}
		static int impossible_rotation_counter = 0;
		static bool impossible_rotation_last = false;
		if((command.angular.x != 0) || (command.angular.y != 0))
		{
			if(impossible_rotation_counter > 40 || !impossible_rotation_last)
			{
				ROS_ERROR("Reaction wheels need alignment. Please reverse polarity on neutron flux capacitor");
				impossible_rotation_counter = 0;
			}
			impossible_rotation_last = true;
			impossible_rotation_counter++;
		}
		else
		{
			impossible_rotation_last = false;
		}
		static int light_speed_counter = 0;
		static bool light_speed_last = false;
		if((sqrt(command.linear.x *command.linear.x + command.linear.y * command.linear.y)) > 300000000)
		{
			if(light_speed_counter > 40 || !light_speed_last)
			{
				ROS_ERROR("PHYSICS VIOLATION DETECTED. DISABLE TELEPORTATION UNIT!");
				light_speed_counter = 0;
			}
			light_speed_last = true;
			light_speed_counter++;
		}
		else
		{
			light_speed_last = false;
		}

		//TODO change to twist msg

		command_struct_.ang = command.angular.z;
		command_struct_.lin[0] = command.linear.x;
		command_struct_.lin[1] = command.linear.y;
		command_struct_.stamp = ros::Time::now();
		command_.writeFromNonRT (command_struct_);

		mode_.writeFromNonRT (true);

#if 0
		//TODO fix debug
		ROS_DEBUG_STREAM_NAMED(name_,
							  "Added values to command. "
							  << "Ang: "   << command_struct_.ang << ", "
							  << "Lin X: "   << command_struct_.lin[0] << ", "
							  << "Lin Y: "   << command_struct_.lin[1] << ", "
							  << "Stamp: " << command_struct_.stamp);
#endif
	}
	else
	{
		ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
	}
}

bool TalonSwerveDriveController::motionProfileService(talon_swerve_drive_controller::MotionProfilePoints::Request &req, talon_swerve_drive_controller::MotionProfilePoints::Response &/*res*/)
{
	if (isRunning())
	{
		/*
		// check that we don't have multiple publishers on the command topic
		if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1)
		{
			ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers()
											<< " publishers. Only 1 publisher is allowed. Going to brake.");
			brake();
			return;
		}
		*/

		//2 Megs -  at least 10 profs

		ROS_WARN("serv points called");

		full_profile_cmd full_profile_struct;
		full_profile_struct.buffer = req.buffer;
		if(req.buffer)
		{
			ROS_INFO_STREAM("size in controller: " << req.profiles.size());
			full_profile_struct.profiles.resize(req.profiles.size());
			for(size_t i = 0; i < req.profiles.size(); i++)
			{
				full_profile_struct.profiles[i].drive_pos.resize(req.profiles[i].points.size());
				full_profile_struct.profiles[i].drive_f.resize(req.profiles[i].points.size());
				full_profile_struct.profiles[i].steer_pos.resize(req.profiles[i].points.size());
				full_profile_struct.profiles[i].steer_f.resize(req.profiles[i].points.size());
				full_profile_struct.profiles[i].hold.resize(req.profiles[i].points.size());
				full_profile_struct.profiles[i].dt = req.profiles[i].dt;
				full_profile_struct.profiles[i].slot = req.profiles[i].slot;
				for(size_t k = 0; k < req.profiles[i].points.size(); k++)
				{
					for(size_t h = 0; h < req.profiles[i].points[k].hold.size(); h++)
					{
						full_profile_struct.profiles[i].hold[k].push_back(req.profiles[i].points[k].hold[h]);
					}
					full_profile_struct.profiles[i].drive_pos[k] = req.profiles[i].points[k].drive_pos;
					full_profile_struct.profiles[i].drive_f[k] = req.profiles[i].points[k].drive_f;
					full_profile_struct.profiles[i].steer_pos[k] = req.profiles[i].points[k].steer_pos;
					full_profile_struct.profiles[i].steer_f[k] = req.profiles[i].points[k].steer_f;
				}
			}
		}

		full_profile_struct.wipe_all		= req.wipe_all;
		full_profile_struct.run				= req.run;
		full_profile_struct.brake			= req.brake;
		full_profile_struct.run_slot		= req.run_slot;
		full_profile_struct.change_queue	= req.change_queue;
		for(size_t i= 0; i< req.new_queue.size(); i++)
		{
			full_profile_struct.new_queue.push_back(req.new_queue[i]);
		}
		full_profile_struct.newly_set		= true;

		//mutex?
		full_profile_buffer_.push_back(full_profile_struct);

		return true;
	}
	else
	{
		ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
		return false;
	}
}

bool TalonSwerveDriveController::brakeService(std_srvs::Empty::Request &/*req*/, std_srvs::Empty::Response &/*res*/)
{
	if (isRunning())
	{
		brake_struct_.lin[0] = 0;
		brake_struct_.lin[1] = 0;
		brake_struct_.ang = 0;
		brake_struct_.stamp = ros::Time::now();
		ROS_WARN("called in controller");
		command_.writeFromNonRT(brake_struct_);
		mode_.writeFromNonRT (true);

		return true;
	}
	else
	{
		ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
		return false;
	}
}

bool TalonSwerveDriveController::wheelPosService(talon_swerve_drive_controller::WheelPos::Request &/*req*/, talon_swerve_drive_controller::WheelPos::Response &res)
{
	if (isRunning())
	{
		std::array<double, WHEELCOUNT> steer_angles;
		{
			std::lock_guard<std::mutex> lock(steer_angles_mutex_);
			steer_angles = steer_angles_;
		}

		for(int i = 0; i < WHEELCOUNT; i++)
		{
			res.positions.push_back(steer_angles[i]);
		}

		return true;
	}
	else
	{
		ROS_ERROR_NAMED(name_, "Can't distribute data. Controller is not running.");
		return false;
	}
}
/*
void TalonSwerveDriveController::cmdCallback(const talon_swerve_drive_controller::CompleteCmd &command)
{
	if (isRunning())
	{
		// check that we don't have multiple publishers on the command topic
		if (sub_command_.getNumPublishers() > 1)
		{
			ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers()
											<< " publishers. Only 1 publisher is allowed. Going to brake.");
			brake();
			return;
		}
		mode_.writeFromNonRT(command.cmd_vel_or_points);
		if(command.cmd_vel_or_points)
		{
			command_struct_.ang = command.twist_.angular.z;
			command_struct_.lin[0] = command.twist_.linear.x;
			command_struct_.lin[1] = command.twist_.linear.y;
			command_struct_.stamp = ros::Time::now();
			command_.writeFromNonRT (command_struct_);
		}
		else
		{
			points_struct_.lin_points_pos.clear();
			points_struct_.lin_points_vel.clear();
			points_struct_.ang_pos.clear();
			points_struct_.ang_vel.clear();
			double duration = command.joint_trajectory.points[1].time_from_start.toSec()
			- command.joint_trajectory.points[0].time_from_start.toSec();

			if(duration < .0025)
			{
			     points_struct_.dt = hardware_interface::TrajectoryDuration::TrajectoryDuration_0ms;
			}
			else if(duration < .0075)
			{
			     points_struct_.dt = hardware_interface::TrajectoryDuration::TrajectoryDuration_5ms;
			}
			else if(duration < .015)
			{
			     points_struct_.dt = hardware_interface::TrajectoryDuration::TrajectoryDuration_10ms;
			}
			else if(duration < .025)
			{
			     points_struct_.dt = hardware_interface::TrajectoryDuration::TrajectoryDuration_20ms;
			}
			else if(duration < .035)
			{
			     points_struct_.dt = hardware_interface::TrajectoryDuration::TrajectoryDuration_30ms;
			}
			else if(duration < .045)
			{
			     points_struct_.dt = hardware_interface::TrajectoryDuration::TrajectoryDuration_40ms;
			}
			else if(duration < .075)
			{
			     points_struct_.dt = hardware_interface::TrajectoryDuration::TrajectoryDuration_50ms;
			}
			else
			{
			     points_struct_.dt = hardware_interface::TrajectoryDuration::TrajectoryDuration_100ms;
			}
			for(size_t i = 0; i < command.joint_trajectory.points.size(); i++)
			{
				points_struct_.lin_points_pos.push_back({command.joint_trajectory.points[i].positions[0], command.joint_trajectory.points[i].positions[1]});
				points_struct_.lin_points_vel.push_back({command.joint_trajectory.points[i].velocities[0], command.joint_trajectory.points[i].velocities[1]});
				points_struct_.ang_pos.push_back(command.joint_trajectory.points[i].positions[2]);
				points_struct_.ang_vel.push_back(command.joint_trajectory.points[i].velocities[2]);
			}
			command_points_.writeFromNonRT(points_struct_);
		}
	}
	else
	{
		ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
	}
}

*/

bool TalonSwerveDriveController::getWheelNames(ros::NodeHandle &controller_nh,
		const std::string &wheel_param,
		std::vector<std::string> &wheel_names)
{
	XmlRpc::XmlRpcValue wheel_list;
	if (!controller_nh.getParam(wheel_param, wheel_list))
	{
		ROS_ERROR_STREAM_NAMED(name_,
							   "Couldn't retrieve wheel param '" << wheel_param << "'.");
		return false;
	}

	if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
	{
		if (wheel_list.size() == 0)
		{
			ROS_ERROR_STREAM_NAMED(name_,
								   "Wheel param '" << wheel_param << "' is an empty list");
			return false;
		}

		for (int i = 0; i < wheel_list.size(); ++i)
		{
			if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
			{
				ROS_ERROR_STREAM_NAMED(name_,
									   "Wheel param '" << wheel_param << "' #" << i <<
									   " isn't a string.");
				return false;
			}
		}

		wheel_names.resize(wheel_list.size());
		for (int i = 0; i < wheel_list.size(); ++i)
		{
			wheel_names[i] = static_cast<std::string>(wheel_list[i]);
		}
	}
	else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
	{
		wheel_names.push_back(wheel_list);
	}
	else
	{
		ROS_ERROR_STREAM_NAMED(name_,
							   "Wheel param '" << wheel_param <<
							   "' is neither a list of strings nor a string.");
		return false;
	}

	return true;
}
}
/*
  bool TalonSwerveDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
                             const std::string& steering_name,
                             const std::string& speed_name,
                             bool lookup_wheel_radius)
  //{
    if (!(lookup_wheel_radius))
    {
      // Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF
      return true;
    }

    // Parse robot description
    const std::string model_param_name = "robot_description";
    bool res = root_nh.hasParam(model_param_name);
    std::string robot_model_str="";
    if (!res || !root_nh.getParam(model_param_name,robot_model_str))
    {
      ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
      return false;
    }

    urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str));

    //TODO: replace with swerve equivalent
    //urdf::JointConstSharedPtr left_wheel_joint(model->getJoint(left_wheel_name));
    //urdf::JointConstSharedPtr right_wheel_joint(model->getJoint(right_wheel_name));

    if(lookup_wheel_radius)
    {
      // Get wheel radius
      if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_))
      {
        ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius");
        return false;
      }
    XmlRpc::XmlRpcValue twist_cov_list;
    controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
    ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
    ROS_ASSERT(twist_cov_list.size() == 6);
    for (int i = 0; i < twist_cov_list.size(); ++i)
      ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);

    // Setup odometry realtime publisher + odom message constant fields
    odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
    odom_pub_->msg_.header.frame_id = odom_frame_id_;
    odom_pub_->msg_.child_frame_id = base_frame_id_;
    odom_pub_->msg_.pose.pose.position.z = 0;
    odom_pub_->msg_.pose.covariance = boost::assign::list_of
        (static_cast<double>(pose_cov_list[0])) (0)  (0)  (0)  (0)  (0)
        (0)  (static_cast<double>(pose_cov_list[1])) (0)  (0)  (0)  (0)
        (0)  (0)  (static_cast<double>(pose_cov_list[2])) (0)  (0)  (0)
        (0)  (0)  (0)  (static_cast<double>(pose_cov_list[3])) (0)  (0)
        (0)  (0)  (0)  (0)  (static_cast<double>(pose_cov_list[4])) (0)
        (0)  (0)  (0)  (0)  (0)  (static_cast<double>(pose_cov_list[5]));
    odom_pub_->msg_.twist.twist.linear.z  = 0;
    odom_pub_->msg_.twist.twist.angular.x = 0;
    odom_pub_->msg_.twist.twist.angular.y = 0;
    odom_pub_->msg_.twist.covariance = boost::assign::list_of
        (static_cast<double>(twist_cov_list[0])) (0)  (0)  (0)  (0)  (0)
        (0)  (static_cast<double>(twist_cov_list[1])) (0)  (0)  (0)  (0)
        (0)  (0)  (static_cast<double>(twist_cov_list[2])) (0)  (0)  (0)
        (0)  (0)  (0)  (static_cast<double>(twist_cov_list[3])) (0)  (0)
        (0)  (0)  (0)  (0)  (static_cast<double>(twist_cov_list[4])) (0)
        (0)  (0)  (0)  (0)  (0)  (static_cast<double>(twist_cov_list[5]));
    tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
    tf_odom_pub_->msg_.transforms.resize(1);
    tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
    tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
    tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
  }
*/
//}


text:
#include <atomic>
#include <boost/noncopyable.hpp>
#include <functional>
#include <memory>
#include <mutex>
#include <pthread.h>

namespace adl {
namespace learn_from_muduo {
/*
 *WARN:没析构呢
 */
/* 多个线程只会初始化一次Singleton<T>，
 *就是只会执行一次init,之后直接通过这个类的instance
 *获得全局唯一对象（指定的T类型）
 *可以有多个不同类型的单例全局对象同时存在
 */
template <typename T> class ThreadLocal_Singleton : boost::noncopyable {
public:
  static T &instance() {
    if (!value_)
      init();
    return *value_;
  }
  ThreadLocal_Singleton() = delete;
  ~ThreadLocal_Singleton() = delete;

private:
  static void init() { value_ = new T(); }
  static pthread_once_t ponce_once;
  static thread_local T *value_;
};

template <typename T>
pthread_once_t ThreadLocal_Singleton<T>::ponce_once =
    PTHREAD_ONCE_INIT; /* 在头文件中就指定了这个类的static变量的初始化 */
template <typename T>
thread_local T *ThreadLocal_Singleton<T>::value_ =
    NULL; /* value就指向一个NULL */
} // namespace learn_from_muduo

namespace learn_from_muduo {
/*
  WARN:未析构
 */
/* 多个线程只会初始化一次Singleton<T>，
 *就是只会执行一次init,之后直接通过这个类的instance
 *获得全局唯一对象（指定的T类型）
 *可以有多个不同类型的单例全局对象同时存在
 */
template <typename T> class Singleton : boost::noncopyable {
public:
  static T &instance() {
    pthread_once(&ponce_once, &Singleton::init);
    return *value_;
  }
  Singleton() = delete;
  ~Singleton() = delete;

private:
  static void init() { value_ = new T(); }
  static pthread_once_t ponce_once;
  static T *value_;
};

template <typename T>
pthread_once_t Singleton<T>::ponce_once =
    PTHREAD_ONCE_INIT; /* 在头文件中就指定了这个类的static变量的初始化 */
template <typename T> T *Singleton<T>::value_ = NULL; /* value就指向一个NULL */
} // namespace learn_from_muduo

namespace use_memory_barrier {
class Singleton : boost::noncopyable {
public:
  static Singleton *getInstance();
  static std::atomic<Singleton *> m_instance;
  static std::mutex m_mutex;
};
std::atomic<Singleton *> Singleton::m_instance;
std::mutex Singleton::m_mutex;

Singleton *Singleton::getInstance() {
  Singleton *tmp = m_instance.load(std::memory_order_relaxed); /* 原子,自由 读*/
  std::atomic_thread_fence(
      std::memory_order_acquire); /* 原子,屏障,acq(后不往前) */
  if (tmp == nullptr) {
    std::lock_guard<std::mutex> lock(m_mutex);
    tmp = m_instance.load(std::memory_order_relaxed); /* 原子,自由,读 */
    if (tmp == nullptr) {
      tmp = new Singleton;
      std::atomic_thread_fence(
          std::memory_order_release); /* 原子,屏障,rel(前不往后) */
      /* 注意到这个前不向后的用途就是tmp如果生成指针重排到构造内容之前，
       *这些写的步骤不会重拍到
       *atomic_thread_fence之后。
       *因此:m_instance会获得到
       *正确构造的数据。
       */
      m_instance.store(tmp, std::memory_order_relaxed); /* 原子,自由,写 */
    }
  }
  return tmp;
}
} // namespace use_memory_barrier

namespace learn_from_starkoverflow {
template <typename T> struct lazy {
public:
  template <typename Fun>
  explicit lazy(Fun &&fun) : fun(std::forward<Fun>(fun)) {}

  T &get() {
    std::call_once(flag, [this] { ptr.reset(fun()); });
    return *ptr;
  }
  // more stuff like op* and op->, implemented in terms of get()

private:
  mutable std::once_flag flag;
  mutable std::unique_ptr<T> ptr;
  std::function<T *()> fun;
};
} // namespace learn_from_starkoverflow
namespace learn_from_csdn {
template <typename T> class Singleton {
private:
  static std::atomic<T *> m_instance;
  static std::mutex m_mutex;

public:
  static T *getInstance();
};

template <typename T> std::atomic<T *> Singleton<T>::m_instance;
template <typename T> std::mutex Singleton<T>::m_mutex;

template <typename T> T *Singleton<T>::getInstance() {
  T *tmp = m_instance.load(std::memory_order_acquire);
  // atomic_thread_fence (std::memory_order_acquire);
  if (tmp == nullptr) {
    std::lock_guard<std::mutex> lock(m_mutex);
    tmp =m_instance.load(std::memory_order_relaxed);
    if (tmp == nullptr) {
      tmp = new T;
      // atomic_thread_fence  (std::memory_order_release);
      m_instance.store(tmp, std::memory_order_release);
    }
  }
  return tmp;
}
} // namespace learn_from_csdn

namespace learn_from_starkoverflow {
/* c++11就可以不用考虑这些问题了. */
template <typename T> class Singleton {
public:
  Singleton(Singleton const &) = delete;
  Singleton &operator=(Singleton const &) = delete;

  static std::shared_ptr<T> instance() {
    static std::shared_ptr<T> s{new T()};
    return s;
  }

  Singleton() = delete;
  ~Singleton() = delete;
};
} // namespace learn_from_starkoverflow
} // namespace adl

ext: hpp

max_stars_repo_name: adlternative/adlServer

max_issues_repo_name: adlternative/adlServer

max_forks_repo_name: adlternative/adlServer


text:
#include "catalog_manager.h"

#include <fstream>

#include <boost/filesystem.hpp>

using namespace std;

//=======================CatalogManager=======================//

CatalogManager::CatalogManager(std::string p) : path_(p) { ReadArchiveFile(); }

CatalogManager::~CatalogManager() { WriteArchiveFile(); }

void CatalogManager::ReadArchiveFile() {
	std::string file_name = path_ + "catalog";
	boost::filesystem::path file_path(file_name);

	file_path.imbue(std::locale("en_US.UTF-8"));

	if (boost::filesystem::exists(file_path)) {
		std::ifstream ifs;
		ifs.open(file_name.c_str(), std::ios::binary);
		boost::archive::binary_iarchive iar(ifs);
		iar >> (*this);
		ifs.close();
	}
}

void CatalogManager::WriteArchiveFile() {
	std::string file_name = path_ + "catalog";

	std::ofstream ofs;
	ofs.open(file_name.c_str(), std::ios::binary);
	boost::archive::binary_oarchive oar(ofs);
	oar << (*this);
	ofs.close();
}

void CatalogManager::CreateDatabase(std::string dbname) {
	dbs_.push_back(Database(dbname));
}

void CatalogManager::DeleteDatabase(std::string dbname) {
	for (unsigned int i = 0; i < dbs_.size(); i++) {
		if (dbs_[i].db_name() == dbname) {
			dbs_.erase(dbs_.begin() + i);
		}
	}
}

Database* CatalogManager::GetDB(std::string db_name) {
	for (unsigned int i = 0; i < dbs_.size(); ++i) {
		if (dbs_[i].db_name() == db_name) {
			return &dbs_[i];
		}
	}
	return NULL;
}

//=======================Database=============================//

Database::Database(std::string dbname) : db_name_(dbname) {}

void Database::CreateTable(SQLCreateTable& st) {
	int record_length = 0;
	Table tb;
	for (int i = 0; i < st.attrs().size(); ++i) {
		tb.AddAttribute(st.attrs()[i]);
		record_length += st.attrs()[i].length();
	}
	tb.set_tb_name(st.tb_name());
	tb.set_record_length(record_length);
	tbs_.push_back(tb);
}

void Database::DropTable(SQLDropTable& st) {
	vector<Table>::iterator i;
	for (i = tbs_.begin(); i != tbs_.end(); i++) {
		if (i->tb_name() == st.tb_name()) {
			tbs_.erase(i);
			return;
		}
	}
}

void Database::DropIndex(SQLDropIndex& st) {
	for (int i = 0; i < tbs_.size(); i++) {
		for (vector<Index>::iterator j = tbs_[i].ids().begin();
			j < tbs_[i].ids().end(); j++) {
			if (j->name() == st.idx_name()) {
				tbs_[i].ids().erase(j);
				return;
			}
		}
	}
}

Table* Database::GetTable(std::string tb_name) {
	for (unsigned int i = 0; i < tbs_.size(); ++i) {
		if (tbs_[i].tb_name() == tb_name) {
			return &tbs_[i];
		}
	}
	return NULL;
}

bool Database::CheckIfIndexExists(std::string index_name) {
	bool exists = false;
	for (unsigned int i = 0; i < tbs_.size(); ++i) {
		for (unsigned int j = 0; j < tbs_[i].ids().size(); ++j) {
			if (tbs_[i].ids()[j].name() == index_name) {
				exists = true;
			}
		}
	}
	return exists;
}

//=======================Table===============================//

Attribute* Table::GetAttribute(std::string name) {
	for (unsigned int i = 0; i < ats_.size(); ++i) {
		if (ats_[i].attr_name() == name) {
			return &ats_[i];
		}
	}
	return NULL;
}

int Table::GetAttributeIndex(std::string name) {
	for (unsigned int i = 0; i < ats_.size(); ++i) {
		if (ats_[i].attr_name() == name) {
			return i;
		}
	}
	return -1;
}

ext: cpp

max_stars_repo_name: JingzeWang/Library-Management-System

max_issues_repo_name: JingzeWang/Library-Management-System

max_forks_repo_name: JingzeWang/Library-Management-System


text:
// Copyright (c) 2021 Bitcoin Association
// Distributed under the Open BSV software license, see the accompanying file LICENSE.

#include "frozentxo_db.h"

#include "uint256.h"
#include "arith_uint256.h"

#include "test/test_bitcoin.h"
#include <boost/test/unit_test.hpp>

#include <random>
#include <set>
#include <thread>

BOOST_FIXTURE_TEST_SUITE(frozentxo, TestingSetup)

namespace {

class RandomTXOGenerator
{
    std::mt19937 engine;
    std::uniform_int_distribution<unsigned int> uniform_dist{0, 255};

public:
    void ResetSeed(decltype(engine)::result_type seed)
    {
        this->engine.seed(seed);
    }

    COutPoint GenerateRandomTXOId()
    {
        uint256 txid;
        for(auto& b: txid)
        {
            b = static_cast<std::uint8_t>(this->uniform_dist(this->engine));
        }
        return COutPoint(txid, this->uniform_dist(engine));
    };
};

} // anonymous namespace

//
// Performs tests on CFrozenTXODB class
//
BOOST_AUTO_TEST_CASE(db_tests)
{
    // Double initialization should throw an exception
    // Note that database was already initialized by TestingSetup constructor
    BOOST_REQUIRE_THROW( CFrozenTXODB::Init(0), std::logic_error );

    auto& db = CFrozenTXODB::Instance();

    COutPoint txo1(uint256S("bbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbb"), 123);
    const auto ftd_po = []{ // Convenient value of FrozenTXOData for TXOs frozen on PolicyOnly blacklist
        CFrozenTXODB::FrozenTXOData ftd = CFrozenTXODB::FrozenTXOData::Create_Uninitialized();
        ftd.blacklist = CFrozenTXODB::FrozenTXOData::Blacklist::PolicyOnly;
        return ftd;
    }();

    COutPoint txo2(uint256S("cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc"), 456);
    const auto ftd_con = [](std::int32_t nHeight){ // Convenient value of FrozenTXOData for TXOs frozen on Consensus blacklist from given height
        CFrozenTXODB::FrozenTXOData ftd = CFrozenTXODB::FrozenTXOData::Create_Uninitialized();;
        ftd.blacklist = CFrozenTXODB::FrozenTXOData::Blacklist::Consensus;
        ftd.enforceAtHeight = { { nHeight } };
        ftd.policyExpiresWithConsensus = false;
        return ftd;
    };

    // FrozenTXOData with invalid value
    const auto ftd0 = []{
        CFrozenTXODB::FrozenTXOData ftd = CFrozenTXODB::FrozenTXOData::Create_Uninitialized();
        ftd.blacklist = static_cast<CFrozenTXODB::FrozenTXOData::Blacklist>(0);
        return ftd;
    }();

    // Used to store FrozenTXOData object set by methods
    CFrozenTXODB::FrozenTXOData ftd = ftd0;

    // Check that FrozenTXOData is correctly considered frozen/unfrozen at specific heights
    auto test_heights = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 123, -1, -2, -3, -4, -5, -6, -7, -8, -9, -123};
    for(std::int32_t h: test_heights )
    {
        // TXO frozen on PolicyOnly blacklist must be considered frozen at any height
        BOOST_CHECK( ftd_po.IsFrozenOnPolicy(h) );

        // Check TXOs frozen on Consensus blacklist
        for(std::int32_t h2: test_heights )
        {
            // Must be considered frozen on PolicyOnly at any height
            BOOST_CHECK( ftd_con(h).IsFrozenOnPolicy(h2) );

            if(h2<h)
            {
                // Must not be considered frozen on Consensus for heights before h
                BOOST_CHECK( !ftd_con(h).IsFrozenOnConsensus(h2) );
            }
            else
            {
                // Must be considered frozen on Consensus for heights h or after
                BOOST_CHECK( ftd_con(h).IsFrozenOnConsensus(h2) );
            }

            // If start >= stop, TXO must not be considered frozen on consensus at any height
            auto ftd = ftd_con(h);
            ftd.enforceAtHeight[0].stop = ftd.enforceAtHeight[0].start;
            BOOST_CHECK( !ftd.IsFrozenOnConsensus(h2) );
            BOOST_CHECK( ftd.IsFrozenOnPolicy(h2) ); // must still be considered frozen on policy ...
            ftd.policyExpiresWithConsensus = true;
            BOOST_CHECK( !ftd.IsFrozenOnPolicy(h2) ); // ... unless policy expires together with consensus

            // If start < stop, TXO must be considered frozen only at heights between start and stop
            ftd = ftd_con(h);
            ftd.enforceAtHeight[0].stop = ftd.enforceAtHeight[0].start+2;
            if(h2<h || h2>=ftd.enforceAtHeight[0].stop)
            {
                BOOST_CHECK( !ftd.IsFrozenOnConsensus(h2) );
            }
            else
            {
                BOOST_CHECK( ftd.IsFrozenOnConsensus(h2) );
            }

            if(h2>=ftd.enforceAtHeight[0].stop)
            {
                BOOST_CHECK( ftd.IsFrozenOnPolicy(h2) ); // must be considered frozen on policy after stop height ...
                ftd.policyExpiresWithConsensus = true;
                BOOST_CHECK( !ftd.IsFrozenOnPolicy(h2) ); // ... unless policy expires together with consensus
            }
            else
            {
                BOOST_CHECK( ftd.IsFrozenOnPolicy(h2) ); // must be considered frozen on policy before stop height ...
                ftd.policyExpiresWithConsensus = true;
                BOOST_CHECK( ftd.IsFrozenOnPolicy(h2) ); // ... even if policy expires together with consensus
            }

            // Check multiple consensus freeze intervals
            ftd = ftd_con(h);
            ftd.enforceAtHeight = {{h, h+2}, {h+4, h+6}, {h+5, h+7}, {h+8, h+8}}; // three valid intervals (two overlapping) and one ignored interval

            // Must be considered frozen on consensus only at heights [h,h+2) and [h+4,h+7).
            if( (h2>=h && h2<h+2) || (h2>=h+4 && h2<h+7) )
            {
                BOOST_CHECK( ftd.IsFrozenOnConsensus(h2) );
            }
            else
            {
                BOOST_CHECK( !ftd.IsFrozenOnConsensus(h2) );
            }

            if(h2>=h+7)
            {
                BOOST_CHECK( ftd.IsFrozenOnPolicy(h2) ); // must be considered frozen on policy after the end of last valid interval ...
                ftd.policyExpiresWithConsensus = true;
                BOOST_CHECK( !ftd.IsFrozenOnPolicy(h2) ); // ... unless policy expires together with consensus
            }
            else
            {
                BOOST_CHECK( ftd.IsFrozenOnPolicy(h2) ); // must be considered frozen on policy before the end of last valid interval ...
                ftd.policyExpiresWithConsensus = true;
                BOOST_CHECK( ftd.IsFrozenOnPolicy(h2) ); // ... even if policy expires together with consensus. note that this includes any gaps between intervals
            }
        }
    }

    // In empty DB txo1 must not be frozen and ftd must not be changed
    BOOST_CHECK( !db.GetFrozenTXOData(txo1, ftd) && ftd==ftd0 );

    // Add a new frozen TXO txo1 to DB
    BOOST_CHECK( db.FreezeTXOPolicyOnly(txo1)==CFrozenTXODB::FreezeTXOResult::OK );

    // txo1 must now be frozen and correct data must be returned
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_po );

    // Freezing the same TXO again must do nothing
    BOOST_CHECK( db.FreezeTXOPolicyOnly(txo1)==CFrozenTXODB::FreezeTXOResult::OK_ALREADY_FROZEN );
    ftd = ftd0;
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_po );

    // txo2 must not be frozen and ftd must remain unchanged if call to GetFrozenTXOData() returns false
    ftd = ftd0;
    BOOST_CHECK( !db.GetFrozenTXOData(txo2, ftd) && ftd==ftd0 );

    // Add a new consensus frozen TXO txo2 to DB
    BOOST_CHECK( db.FreezeTXOConsensus(txo2, {{0}}, false)==CFrozenTXODB::FreezeTXOResult::OK );

    // txo2 must now be frozen
    ftd=ftd0;
    BOOST_CHECK( db.GetFrozenTXOData(txo2, ftd) && ftd==ftd_con(0) );

    // txo1 must still be frozen
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_po );

    // Freezing the same TXO again must do nothing
    BOOST_CHECK( db.FreezeTXOConsensus(txo2, {{0}}, false)==CFrozenTXODB::FreezeTXOResult::OK_ALREADY_FROZEN );
    ftd = ftd0;
    BOOST_CHECK( db.GetFrozenTXOData(txo2, ftd) && ftd==ftd_con(0) );

    // Update blacklist on txo1 to consensus
    BOOST_CHECK( db.FreezeTXOConsensus(txo1, {{0}}, false)==CFrozenTXODB::FreezeTXOResult::OK_UPDATED_TO_CONSENSUS_BLACKLIST );

    // txo1 must still be frozen and updated data must be returned
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_con(0) );

    // Updating from consensus to policyOnly is not allowed
    BOOST_CHECK( db.FreezeTXOPolicyOnly(txo1)==CFrozenTXODB::FreezeTXOResult::ERROR_ALREADY_IN_CONSENSUS_BLACKLIST );

    // txo1 must still be frozen and original data must be returned
    ftd=ftd0;
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_con(0) );

    // Update start height on txo1
    BOOST_CHECK( db.FreezeTXOConsensus(txo1, {{2}}, false)==CFrozenTXODB::FreezeTXOResult::OK_UPDATED );
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_con(2) );

    // Change enforceAtHeight to several intervals to check that it is correctly serialized
    BOOST_CHECK( db.FreezeTXOConsensus(txo1, {{2,3}, {4,5}}, false)==CFrozenTXODB::FreezeTXOResult::OK_UPDATED );
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) );
    {
        auto ftd_chk=ftd_con(2);
        ftd_chk.enforceAtHeight = {{2,3}, {4,5}};
        BOOST_CHECK( ftd==ftd_chk );
    }
    BOOST_CHECK( db.FreezeTXOConsensus(txo1, {{4,5}, {-1,-1}, {2,3}}, true)==CFrozenTXODB::FreezeTXOResult::OK_UPDATED );
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) );
    {
        auto ftd_chk=ftd_con(2);
        ftd_chk.enforceAtHeight = {{4,5}, {-1,-1},  {2,3}};
        ftd_chk.policyExpiresWithConsensus = true;
        BOOST_CHECK( ftd==ftd_chk );
    }

    // Update start height on txo1 back to 0 as is expected for next steps
    BOOST_CHECK( db.FreezeTXOConsensus(txo1, {{0}}, false)==CFrozenTXODB::FreezeTXOResult::OK_UPDATED );
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_con(0) );

    // Unfreezing TXO that is in Consensus from PolicyOnly is not allowed
    BOOST_CHECK( db.UnfreezeTXOPolicyOnly(txo1)==CFrozenTXODB::UnfreezeTXOResult::ERROR_TXO_IS_IN_CONSENSUS_BLACKLIST );
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_con(0) );

    // Unfreeze txo1 from Consensus at height 2 but keep it frozen on PolicyOnly
    BOOST_CHECK( db.FreezeTXOConsensus(txo1, {{0,2}}, false)==CFrozenTXODB::FreezeTXOResult::OK_UPDATED );

    // txo1 must still be stored on consensus blacklist with stop height set accordingly
    BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd.blacklist == CFrozenTXODB::FrozenTXOData::Blacklist::Consensus );
    BOOST_CHECK( ftd.enforceAtHeight[0].stop==2 );
    BOOST_CHECK( ftd.policyExpiresWithConsensus==false );

    // Unfreeze txo2 from consensus and policy at height 3
    BOOST_CHECK( db.FreezeTXOConsensus(txo2, {{0,3}}, true)==CFrozenTXODB::FreezeTXOResult::OK_UPDATED );

    // txo2 must still be stored on consensus blacklist with stop height set accordingly
    BOOST_CHECK( db.GetFrozenTXOData(txo2, ftd) && ftd.blacklist == CFrozenTXODB::FrozenTXOData::Blacklist::Consensus );
    BOOST_CHECK( ftd.enforceAtHeight[0].stop==3 );
    BOOST_CHECK( ftd.policyExpiresWithConsensus==true );

    {
        // Remove all TXO records and add a new one needed for next check
        auto res = db.UnfreezeAll();
        BOOST_CHECK( res.numUnfrozenPolicyOnly==0 );
        BOOST_CHECK( res.numUnfrozenConsensus==2 );
        BOOST_CHECK( db.FreezeTXOPolicyOnly(txo1)==CFrozenTXODB::FreezeTXOResult::OK );
    }

    // Unfreezing TXO that is currently in PolicyOnly removes record from DB
    BOOST_CHECK( db.UnfreezeTXOPolicyOnly(txo1)==CFrozenTXODB::UnfreezeTXOResult::OK );
    BOOST_CHECK( !db.GetFrozenTXOData(txo1, ftd) );

    // Trying to unfreeze TXO that is not frozen is not allowed
    BOOST_CHECK( db.UnfreezeTXOPolicyOnly(txo1)==CFrozenTXODB::UnfreezeTXOResult::ERROR_TXO_NOT_FROZEN );

    // Check cleaning expired records from DB.
    // Note that at this point, there must be no records in DB.
    BOOST_CHECK( db.FreezeTXOConsensus(txo1, {{1,2}}, true)==CFrozenTXODB::FreezeTXOResult::OK );
    {
        // Must not be considered expired at height 0.
        auto res = db.CleanExpiredRecords(0);
        BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && res.numConsensusRemoved==0 && res.numConsensusUpdatedToPolicyOnly==0 );
        // Must not be considered expired at height 1.
        res = db.CleanExpiredRecords(1);
        BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && res.numConsensusRemoved==0 && res.numConsensusUpdatedToPolicyOnly==0 );
        // Must be considered expired and removed at height 2.
        res = db.CleanExpiredRecords(2);
        BOOST_CHECK( !db.GetFrozenTXOData(txo1, ftd) && res.numConsensusRemoved==1 && res.numConsensusUpdatedToPolicyOnly==0 );
    }

    BOOST_CHECK( db.FreezeTXOConsensus(txo1, {{1,2}}, false)==CFrozenTXODB::FreezeTXOResult::OK );
    {
        // Must not be considered expired at height 0.
        auto res = db.CleanExpiredRecords(0);
        BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && res.numConsensusRemoved==0 && res.numConsensusUpdatedToPolicyOnly==0 );
        // Must not be considered expired at height 1.
        res = db.CleanExpiredRecords(1);
        BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && res.numConsensusRemoved==0 && res.numConsensusUpdatedToPolicyOnly==0 );
        // Must be considered expired and updated to policy at height 2.
        res = db.CleanExpiredRecords(2);
        BOOST_CHECK( db.GetFrozenTXOData(txo1, ftd) && ftd==ftd_po && res.numConsensusRemoved==0 && res.numConsensusUpdatedToPolicyOnly==1 );
    }

    BOOST_CHECK( db.FreezeTXOConsensus(txo2, {{1,1}}, true)==CFrozenTXODB::FreezeTXOResult::OK );
    {
        // Must be considered expired and removed at any height.
        auto res = db.CleanExpiredRecords(1);
        BOOST_CHECK( !db.GetFrozenTXOData(txo2, ftd) && res.numConsensusRemoved==1 && res.numConsensusUpdatedToPolicyOnly==0 );
    }

    // Remove remaining txo1 record because next step expects empty DB
    BOOST_CHECK( db.UnfreezeTXOPolicyOnly(txo1)==CFrozenTXODB::UnfreezeTXOResult::OK );

    // Iterator must not be valid if there are no frozen TXOs
    BOOST_CHECK( !db.QueryAllFrozenTXOs().Valid() );

    RandomTXOGenerator rtg;

    using clock = std::chrono::steady_clock;

    // Add some TXOs to check iteration and bulk operations
    rtg.ResetSeed(1); // fixed seed is used so that results are always the same
    const unsigned int N = 10000;
    const unsigned int N2 = N/2;
    auto tp0=clock::now();
    const arith_uint256 txid_sum = [&]{
        arith_uint256 ts;
        for(unsigned int i=0; i<N; ++i)
        {
            auto txoid = rtg.GenerateRandomTXOId();
            BOOST_CHECK( db.FreezeTXOPolicyOnly(txoid)==CFrozenTXODB::FreezeTXOResult::OK );

            // update sum that will later be used to check if all TXOs are returned by iteration
            ts += UintToArith256(txoid.GetTxId());
            ts += txoid.GetN();
        }
        return ts;
    }();
    db.Sync();
    BOOST_TEST_MESSAGE( "Freezing "+std::to_string(N)+ " TXOs took "+std::to_string(std::chrono::duration_cast<std::chrono::milliseconds>(clock::now()-tp0 ).count())+"ms" );

    // Check that all added TXO are frozen and time the result
    rtg.ResetSeed(1); // setting the same seed as above so that random generator will once again generate same sequence of COutPoint's as in the previous run
    tp0=clock::now();
    for(unsigned int i=0; i<N; ++i)
    {
        auto txoid = rtg.GenerateRandomTXOId();
        BOOST_CHECK( db.GetFrozenTXOData(txoid, ftd) && ftd.IsFrozenOnPolicy(0) );
    }
    BOOST_TEST_MESSAGE( "Checking that TXO is frozen "+std::to_string(N)+ " times took "+std::to_string(std::chrono::duration_cast<std::chrono::milliseconds>(clock::now()-tp0 ).count())+"ms" );

    tp0=clock::now();
    for(unsigned int i=0; i<N; ++i)
    {
        auto txoid = rtg.GenerateRandomTXOId(); // since seed was not reset, new COutPoint's are generated that are not frozen
        BOOST_CHECK( !db.GetFrozenTXOData(txoid, ftd) );
    }
    BOOST_TEST_MESSAGE( "Checking that TXO is not frozen "+std::to_string(N)+ " times took "+std::to_string(std::chrono::duration_cast<std::chrono::milliseconds>(clock::now()-tp0 ).count())+"ms" );

    // Next random TXO should not be frozen
    BOOST_CHECK( !db.GetFrozenTXOData(rtg.GenerateRandomTXOId(), ftd) );

    // Check iteration over all frozen TXOs
    std::set<COutPoint> txoid_front;
    COutPoint txoid_last;
    unsigned int cnt=0;
    arith_uint256 txid_sum_chk = 0;
    for(auto it=db.QueryAllFrozenTXOs(); it.Valid(); it.Next())
    {
        auto t = it.GetFrozenTXO();
        BOOST_CHECK( t.second == ftd_po );

        // Remember first N2 and the last TXO id as they are stored in DB so that can be used in next test
        if(cnt<N2)
        {
            txoid_front.insert(t.first);
        }
        else if(cnt+1==N)
        {
            txoid_last = t.first;
        }

        ++cnt;
        txid_sum_chk += UintToArith256(t.first.GetTxId());
        txid_sum_chk += t.first.GetN();
    }
    BOOST_CHECK( cnt==N );
    BOOST_CHECK( txid_sum_chk==txid_sum );
    BOOST_CHECK( txoid_front.size()==N2 );

    // Check that modifying DB while iterating produces predictable results
    cnt=0;
    txid_sum_chk = 0;
    for(auto it=db.QueryAllFrozenTXOs(); it.Valid(); it.Next())
    {
        if(cnt==2)
        {
            // Modify DB by unfreezing some TXOs before and after current position
            // In total N2+1 TXOs are unfrozen.
            // NOTE: This is done from another thread because this is a typical scenario
            std::thread thd([&db, &txoid_front, &txoid_last]{
                for(auto& txoid: txoid_front)
                {
                    BOOST_CHECK( db.UnfreezeTXOPolicyOnly(txoid)==CFrozenTXODB::UnfreezeTXOResult::OK );
                }
                BOOST_CHECK( db.UnfreezeTXOPolicyOnly(txoid_last)==CFrozenTXODB::UnfreezeTXOResult::OK );

                // Check that records are actually removed from DB

                CFrozenTXODB::FrozenTXOData ftd = CFrozenTXODB::FrozenTXOData::Create_Uninitialized();
                for(auto& txoid: txoid_front)
                {
                    BOOST_CHECK( !db.GetFrozenTXOData(txoid, ftd) );
                }
                BOOST_CHECK( !db.GetFrozenTXOData(txoid_last, ftd) );
            });
            thd.join();
        }

        // Even if some records were removed above in a separate thread, we should still get all of them when using iterator that was created before
        auto t = it.GetFrozenTXO();
        BOOST_CHECK( t.second == ftd_po );

        ++cnt;
        txid_sum_chk += UintToArith256(t.first.GetTxId());
        txid_sum_chk += t.first.GetN();
    }
    db.Sync();
    BOOST_CHECK( cnt==N );
    BOOST_CHECK( txid_sum_chk==txid_sum );

    // Unfreeze all TXOs that are still frozen
    rtg.ResetSeed(1);
    unsigned int N_removed = 0;
    for(unsigned int i=0; i<N; ++i)
    {
        auto txoid = rtg.GenerateRandomTXOId();
        auto expected_result = CFrozenTXODB::UnfreezeTXOResult::OK;
        if(txoid_front.count(txoid)!=0 || txoid==txoid_last)
        {
            expected_result = CFrozenTXODB::UnfreezeTXOResult::ERROR_TXO_NOT_FROZEN;
        }
        else
        {
            ++N_removed;
        }
        BOOST_CHECK( db.UnfreezeTXOPolicyOnly(txoid)==expected_result );
    }
    db.Sync();
    BOOST_CHECK( N2 + N_removed + 1 == N );
    BOOST_CHECK( !db.QueryAllFrozenTXOs().Valid() );

    // Check UnfreezeAll() method
    BOOST_CHECK( db.FreezeTXOPolicyOnly(txo1)==CFrozenTXODB::FreezeTXOResult::OK );
    BOOST_CHECK( db.FreezeTXOConsensus(txo2, {{0}}, false)==CFrozenTXODB::FreezeTXOResult::OK );
    BOOST_CHECK( db.FreezeTXOConsensus(COutPoint(uint256S("dddddddddddddddddddddddddddddddddddddddddddddddddddddddddddddddd"), 0), {{0}}, false)==CFrozenTXODB::FreezeTXOResult::OK );
    auto res = db.UnfreezeAll();
    BOOST_CHECK( res.numUnfrozenPolicyOnly==1 );
    BOOST_CHECK( res.numUnfrozenConsensus==2 );
    BOOST_CHECK( !db.QueryAllFrozenTXOs().Valid() );
    res = db.UnfreezeAll(); // running on empty db should do nothing
    BOOST_CHECK( res.numUnfrozenPolicyOnly==0 );
    BOOST_CHECK( res.numUnfrozenConsensus==0 );
}

namespace {
// Helper to run function in two threads.
template<typename Cnt, typename F>
void run_in_two_threads(Cnt (&cnt)[2], F f)
{
    cnt[0] = {};
    std::thread thd(f, &cnt[0]);

    cnt[1] = {};
    f(&cnt[1]);

    thd.join();
}
}

//
// Performs thread-safety tests on CFrozenTXODB class
//
// Checking if TXO is frozen must work correctly even if TXO is being frozen/unfrozen at the same time.
// Freezing/unfreezing TXO must work correctly even if it done from several threads at the same time.
//
BOOST_AUTO_TEST_CASE(db_thread_safety_tests)
{
    auto& db = CFrozenTXODB::Instance();

    // Generate array with TXO ids that will be used in the test
    const auto txo_array = []{
        const std::size_t N = 10000;
        std::vector<COutPoint> txo_array(N);

        RandomTXOGenerator rtg;
        rtg.ResetSeed(1); // fixed seed is used so that results are always the same
        for(auto& txo: txo_array)
        {
            txo = rtg.GenerateRandomTXOId();
        }

        return txo_array;
    }();

    std::atomic_bool should_quit(false); // if set to true, thread should quit
    struct CheckerData
    {
        std::size_t num_frozen = -1; // number of frozen TXOs after final loop
    };
    CheckerData policy;
    CheckerData consensus;

    // Helper that continuously checks all TXOs in array and returns when should_quit is true.
    auto frozen_txo_checker = [&](std::int32_t height){
        for(std::size_t i = 0; ; ++i) // NOTE: This loop typically finishes after only two iterations because writing thread takes most of the time.
        {
            // Check quit signal so that we can do one more loop before quitting
            bool should_quit_after_loop = should_quit;

            std::size_t num_policy = 0;
            std::size_t num_consensus = 0;
            for(auto& txo: txo_array)
            {
                CFrozenTXODB::FrozenTXOData ftd = CFrozenTXODB::FrozenTXOData::Create_Uninitialized();
                if(!db.GetFrozenTXOData(txo, ftd))
                {
                    continue;
                }
                if(ftd.IsFrozenOnPolicy(height))
                {
                    ++num_policy;
                }
                if(ftd.IsFrozenOnConsensus(height))
                {
                    ++num_consensus;
                }
            }

            if(should_quit_after_loop)
            {
                // Report number of TXOs that were found frozen in final loop.
                policy.num_frozen = num_policy;
                consensus.num_frozen = num_consensus;
                break;
            }
        }
    };

    // Thread that will check for frozen TXOs
    std::thread thd_checker;
    auto start_frozen_txo_checker = [&](std::int32_t height) {
        policy = CheckerData();
        consensus = CheckerData();
        should_quit = false;
        thd_checker = std::thread(frozen_txo_checker, height);
    };
    auto stop_frozen_txo_checker = [&]{
        should_quit = true;
        thd_checker.join();
    };

    // Provides counters that are used to check if function completed successfully
    struct Cnt
    {
        std::size_t ok = 0;
        std::size_t alt = 0;
    } cnt[2];

    // Freeze all TXOs on policy-only blacklist while checking if they are frozen
    start_frozen_txo_checker(0);
    run_in_two_threads(cnt, [&](Cnt* cnt){
        for(auto& txo: txo_array)
        {
            auto res = db.FreezeTXOPolicyOnly(txo);
            if(res == CFrozenTXODB::FreezeTXOResult::OK)
            {
                ++cnt->ok;
            }
            else if(res == CFrozenTXODB::FreezeTXOResult::OK_ALREADY_FROZEN)
            {
                ++cnt->alt;
            }
        }
    });
    stop_frozen_txo_checker();
    db.Sync();
    BOOST_CHECK_EQUAL( policy.num_frozen, txo_array.size() );
    BOOST_CHECK_EQUAL( consensus.num_frozen, 0 );
    BOOST_CHECK_EQUAL( cnt[0].ok + cnt[0].alt, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[1].ok + cnt[1].alt, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[0].ok + cnt[1].ok, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[0].alt + cnt[1].alt, txo_array.size() );

    // Freeze all TXOs on consensus blacklist at height 10 while checking if they are frozen
    start_frozen_txo_checker(10);
    run_in_two_threads(cnt, [&](Cnt* cnt){
        for(auto& txo: txo_array)
        {
            auto res = db.FreezeTXOConsensus(txo, {{10}}, false);
            if(res == CFrozenTXODB::FreezeTXOResult::OK_UPDATED_TO_CONSENSUS_BLACKLIST)
            {
                ++cnt->ok;
            }
            else if(res == CFrozenTXODB::FreezeTXOResult::OK_ALREADY_FROZEN)
            {
                ++cnt->alt;
            }
        }
    });
    stop_frozen_txo_checker();
    db.Sync();
    BOOST_CHECK_EQUAL( policy.num_frozen, txo_array.size() );
    BOOST_CHECK_EQUAL( consensus.num_frozen, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[0].ok + cnt[0].alt, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[1].ok + cnt[1].alt, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[0].ok + cnt[1].ok, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[0].alt + cnt[1].alt, txo_array.size() );

    // Unfreeze all TXOs on consensus blacklist at height 20 while checking if they are frozen
    start_frozen_txo_checker(20);
    run_in_two_threads(cnt, [&](Cnt* cnt){
        for(auto& txo: txo_array)
        {
            auto res = db.FreezeTXOConsensus(txo, {{10,20}}, false);
            if(res == CFrozenTXODB::FreezeTXOResult::OK_UPDATED)
            {
                ++cnt->ok;
            }
            else if(res == CFrozenTXODB::FreezeTXOResult::OK_ALREADY_FROZEN)
            {
                ++cnt->alt;
            }
        }
    });
    stop_frozen_txo_checker();
    db.Sync();
    BOOST_CHECK_EQUAL( policy.num_frozen, txo_array.size() );
    BOOST_CHECK_EQUAL( consensus.num_frozen, 0 );
    BOOST_CHECK_EQUAL( cnt[0].ok + cnt[0].alt, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[1].ok + cnt[1].alt, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[0].ok + cnt[1].ok, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[0].alt + cnt[1].alt, txo_array.size() );

    // Clear expired records (update to policy) while checking if they are frozen
    start_frozen_txo_checker(20);
    run_in_two_threads(cnt, [&](Cnt* cnt){
        auto res = db.CleanExpiredRecords(20);
        cnt->ok = res.numConsensusUpdatedToPolicyOnly;
        cnt->alt = res.numConsensusRemoved;
    });
    stop_frozen_txo_checker();
    db.Sync();
    BOOST_CHECK_EQUAL( policy.num_frozen, txo_array.size() );
    BOOST_CHECK_EQUAL( consensus.num_frozen, 0 );
    BOOST_CHECK_EQUAL( cnt[0].ok + cnt[1].ok, txo_array.size() );
    BOOST_CHECK_EQUAL( cnt[0].alt, 0 );
    BOOST_CHECK_EQUAL( cnt[1].alt, 0 );

    // Remove all frozen TXO records while checking if they are frozen
    start_frozen_txo_checker(20);
    run_in_two_threads(cnt, [&](Cnt* cnt){
        auto res = db.UnfreezeAll();
        cnt->ok = res.numUnfrozenPolicyOnly;
        cnt->alt = res.numUnfrozenConsensus;
    });
    stop_frozen_txo_checker();
    db.Sync();
    BOOST_CHECK_EQUAL( policy.num_frozen, 0 );
    BOOST_CHECK_EQUAL( consensus.num_frozen, 0 );
    BOOST_CHECK_EQUAL( cnt[0].ok + cnt[1].ok, txo_array.size());
    BOOST_CHECK_EQUAL( cnt[0].alt, 0);
    BOOST_CHECK_EQUAL( cnt[1].alt, 0);
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: ZacharyPainter/bitcoin-sv

max_issues_repo_name: wjxee/bitcoin-sv

max_forks_repo_name: wjxee/bitcoin-sv


text:
/*=============================================================================
    Copyright (c) 2001-2003 Joel de Guzman
    Copyright (c) 2001-2003 Daniel Nuffer
    http://spirit.sourceforge.net/

  Distributed under the Boost Software License, Version 1.0. (See accompanying
  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#ifndef BOOST_SPIRIT_BASIC_CHSET_HPP
#define BOOST_SPIRIT_BASIC_CHSET_HPP

///////////////////////////////////////////////////////////////////////////////
#include <bitset>
#include <boost/spirit/home/classic/utility/impl/chset/range_run.hpp>
#include <boost/spirit/home/classic/namespace.hpp>

namespace boost { namespace spirit {

BOOST_SPIRIT_CLASSIC_NAMESPACE_BEGIN

    ///////////////////////////////////////////////////////////////////////////
    //
    //  basic_chset: basic character set implementation using range_run
    //
    ///////////////////////////////////////////////////////////////////////////
    template <typename CharT>
    class basic_chset
    {
    public:
                            basic_chset();
                            basic_chset(basic_chset const& arg_);

        bool                test(CharT v) const;
        void                set(CharT from, CharT to);
        void                set(CharT c);
        void                clear(CharT from, CharT to);
        void                clear(CharT c);
        void                clear();

        void                inverse();
        void                swap(basic_chset& x);

        basic_chset&        operator|=(basic_chset const& x);
        basic_chset&        operator&=(basic_chset const& x);
        basic_chset&        operator-=(basic_chset const& x);
        basic_chset&        operator^=(basic_chset const& x);

        private: utility::impl::range_run<CharT> rr;
    };

    #if (CHAR_BIT == 8)

    ///////////////////////////////////////////////////////////////////////////
    //
    //  basic_chset: specializations for 8 bit chars using std::bitset
    //
    ///////////////////////////////////////////////////////////////////////////
    template <typename CharT>
    class basic_chset_8bit {

    public:
                            basic_chset_8bit();
                            basic_chset_8bit(basic_chset_8bit const& arg_);

        bool                test(CharT v) const;
        void                set(CharT from, CharT to);
        void                set(CharT c);
        void                clear(CharT from, CharT to);
        void                clear(CharT c);
        void                clear();

        void                inverse();
        void                swap(basic_chset_8bit& x);

        basic_chset_8bit&   operator|=(basic_chset_8bit const& x);
        basic_chset_8bit&   operator&=(basic_chset_8bit const& x);
        basic_chset_8bit&   operator-=(basic_chset_8bit const& x);
        basic_chset_8bit&   operator^=(basic_chset_8bit const& x);

        private: std::bitset<256> bset;
    };

    /////////////////////////////////
    template <>
    class basic_chset<char>
    : public basic_chset_8bit<char> {};

    /////////////////////////////////
    template <>
    class basic_chset<signed char>
    : public basic_chset_8bit<signed char> {};

    /////////////////////////////////
    template <>
    class basic_chset<unsigned char>
    : public basic_chset_8bit<unsigned char> {};

#endif

BOOST_SPIRIT_CLASSIC_NAMESPACE_END

}} // namespace BOOST_SPIRIT_CLASSIC_NS

#endif

#include <boost/spirit/home/classic/utility/impl/chset/basic_chset.ipp>

max_issues_repo_name: cpmech/vismatrix

max_forks_repo_name: cpmech/vismatrix


text:
#ifndef PLANNING_GOAL_CHECKER_HELPER_HPP
#define PLANNING_GOAL_CHECKER_HELPER_HPP

// Component
#include "LocalPlannerData.hpp"

// Libraries
#include <boost/cstdfloat.hpp>

namespace local_planner
{

using float64_t = boost::float64_t; ///< Alias for 64 bit float

/// @brief Class for integrating callback data to a given time
class GoalChecker
{
public:
    /// @brief Determines whether goal has been reached
    /// @param data Local Planner data
    /// @param tol_m Tolerance, in meters
    /// @return `true` if goal has been reached
    static bool checkGoalReached(const LocalPlannerData& data, const float64_t tol_m) noexcept;
};

} // namespace local_planner

#endif // PLANNING_GOAL_CHECKER_HELPER_HPP


text:
#include <fstream>
#include <vector>
#include <string>
#include <stdio.h>

#include <rw/math/Random.hpp>
#include <rw/math/Vector3D.hpp>
#include <rwlibs/task/GraspTask.hpp>

using namespace std;
using rw::core::ownedPtr;
using namespace rw::math;
using namespace rwlibs::task;

const double SOFT_LAYER_SIZE = 0.0005;

void loadPoses(std::string filename, std::vector<Transform3D<> >& initialPoses, std::vector<Transform3D<> >& sampledPoses){
    std::ifstream in;
    in.open(filename.c_str());
    char line[200];

    in.getline(line,200);
    int initialPoseCnt, samplesPerPose;
    sscanf(line, "%i", &initialPoseCnt);
    in.getline(line,200);
    sscanf(line, "%i", &samplesPerPose);

    std::cout << "initialPoseCnt: " << initialPoseCnt << std::endl;
    std::cout << "samplesPerPose: " << samplesPerPose << std::endl;

    float x,y,z,r[9];
    // first we read initial psoes
    for(int i=0;i<initialPoseCnt;i++) {
        in.getline(line,200);
        sscanf(line, "%f %f %f %f %f %f %f %f %f %f %f %f", &x, &y, &z, &r[0], &r[1], &r[2], &r[3], &r[4], &r[5], &r[6], &r[7], &r[8] );
        Rotation3D<> rot(r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7], r[8]);
        rot.normalize();
        initialPoses.push_back( inverse( Transform3D<>(Vector3D<>(x,y,z), rot)));
    }

    // next we read the poses to simulate
    for(int i=0;i<initialPoseCnt*samplesPerPose;i++) {
        in.getline(line,200);
        sscanf(line, "%f %f %f %f %f %f %f %f %f %f %f %f", &x, &y, &z, &r[0], &r[1], &r[2], &r[3], &r[4], &r[5], &r[6], &r[7], &r[8] );
        Rotation3D<> rot(r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7], r[8]);
        rot.normalize();
        sampledPoses.push_back( inverse( Transform3D<>(Vector3D<>(x,y,z), rot)));
    }

}
#include <boost/lexical_cast.hpp>
int main(int argc, char** argv)
{
	static const unsigned int SEED = static_cast<unsigned int>(time(NULL));
	Random::seed(SEED);
	srand(SEED);

    if( argc < 3 ){
		std::cout << "------ Usage: " << std::endl;
	    std::cout << "- Arg 1 name of HGP txt file" << std::endl;
	    std::cout << "- Arg 2 name of output grasp task file" << std::endl;
	    return 0;
	}

	std::string grasptask_file(argv[1]);
	std::string grasptask_file_out( argv[2] );

	std::vector<Transform3D<> > initPoses, samplePoses;
	std::cout << "Loading poses from txt file" << std::endl;
	loadPoses(grasptask_file, initPoses, samplePoses);

	std::cout << "Creating tasks" << std::endl;
	GraspTask::Ptr gtask = ownedPtr( new GraspTask() );
	// add one task with all the targets

	gtask->setGripperID("GS20");
	gtask->setTCPID("TCPGS20");
	gtask->setGraspControllerID("GraspController");

	gtask->getSubTasks().resize(1);
    gtask->getSubTasks()[0].closeQ = Q(1, 0.0005);
    gtask->getSubTasks()[0].openQ = Q(1, 0.005);
    gtask->getSubTasks()[0].retract.P() = Vector3D<>(0,0,-0.04);
	std::vector<GraspTarget>& targets = gtask->getSubTasks()[0].getTargets();
	for(Transform3D<> &t3d : initPoses) {
	    targets.push_back( GraspTarget( t3d ) );
	}

	GraspTask::saveRWTask( gtask, grasptask_file_out+"_init.uibk.xml" );
    //gtask->getSubTasks().resize(1);
    //std::vector<GraspTarget>& targets = gtask->getSubTasks()[0].getTargets();
	int nrPosesWritten=0, nrSaves=0;
    for(Transform3D<> &t3d : samplePoses) {
        targets.push_back( GraspTarget( t3d ) );
        nrPosesWritten++;
        if(nrPosesWritten>30000){
            GraspTask::saveRWTask( gtask, grasptask_file_out+"_"+boost::lexical_cast<std::string>(nrSaves)+"_init_samples.uibk.xml" );
            nrSaves++;
            targets.clear();
            nrPosesWritten = 0;
        }
    }
    GraspTask::saveRWTask( gtask, grasptask_file_out+"_"+boost::lexical_cast<std::string>(nrSaves)+"_init_samples.uibk.xml" );

    std::vector<GraspTarget>& targets2 = gtask->getSubTasks()[0].getTargets();
    gtask->getSubTasks()[0] = GraspSubTask();
    gtask->getSubTasks()[0].closeQ = Q(1, 0.0005);
    gtask->getSubTasks()[0].openQ = Q(1, 0.005);
    gtask->getSubTasks()[0].retract.P() = Vector3D<>(0,0,-0.04);

    nrPosesWritten=0; nrSaves=0;
    for(Transform3D<> &t3d : samplePoses) {
        targets2.push_back( GraspTarget( t3d ) );
        nrPosesWritten++;
        if(nrPosesWritten>30000){
            GraspTask::saveRWTask( gtask, grasptask_file_out+"_"+boost::lexical_cast<std::string>(nrSaves)+"_samples.uibk.xml" );
            nrSaves++;
            targets2.clear();
            nrPosesWritten = 0;
        }
    }
    GraspTask::saveRWTask( gtask, grasptask_file_out+"_"+boost::lexical_cast<std::string>(nrSaves)+"_samples.uibk.xml" );

    return 0;
}


text:
#include <boost/python/class.hpp>
#include <boost/python/implicit.hpp>

#include <smtbx/refinement/constraints/affine.h>

namespace smtbx { namespace refinement { namespace constraints {
  namespace boost_python {

  struct affine_scalar_parameter_wrapper {
    typedef affine_scalar_parameter wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt,
             bases<scalar_parameter>,
             std::auto_ptr<wt> >("affine_scalar_parameter", no_init)
        .def(init<scalar_parameter *, double, double>
             ((arg("dependee"), arg("a"), arg("b"))))
        .def(init<scalar_parameter *, double,
                  scalar_parameter *, double,
                  double>
             ((arg("dependee_0"), arg("a_0"),
               arg("dependee_1"), arg("a_1"),
               arg("b"))))
        .def(init<af::shared<scalar_parameter *> const &,
                  af::shared<double> const &, double>
             ((arg("dependees"), arg("a"), arg("b"))))
        .add_property("affine_form", &wt::affine_form)
        ;
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  void wrap_affine_scalar_parameter_wrapper() {
    affine_scalar_parameter_wrapper::wrap();
  }

}}}}