<commit_before>#include <mutex>
#include <thread>
#include <ros/spinner.h>

#include <ensenso/visualizer.h>
#include <ensenso/pcl_headers.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>

/*Globlal namespaces and aliases*/
// using namespace pathfinder;
using PointT 			= pcl::PointXYZ;
using PointCloudT 		= pcl::PointCloud<PointT>;
using PointCloudTPtr 	= PointCloudT::Ptr;

using PointN  			= pcl::Normal;
using PointCloudN 		= pcl::PointCloud<PointN>;
using PointCloudNPtr  	= PointCloudN::Ptr;

using pcl_viz 			= pcl::visualization::PCLVisualizer;
using NormalEstimation 	= pcl::NormalEstimation<PointT, PointN>;

#define OUT(__o__) std::cout<< __o__ << std::endl;

class Segmentation
{
private:
	bool updateCloud, save, running;
	ros::NodeHandle nh_;
 	const std::string cloudName;
	ros::Subscriber cloud_sub_;
	std::mutex mutex;
 	PointCloudT cloud;
 	std::thread cloudDispThread, normalsDispThread;
 	unsigned long const hardware_concurrency;
 	ros::AsyncSpinner spinner;
 	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer, segViewer;
 	boost::shared_ptr<pcl::visualization::PCLVisualizer> multiViewer;
 	NormalEstimation normalEstimation;
	PointCloudNPtr normals;

	boost::shared_ptr<visualizer> viz;
	std::vector<std::thread> threads;
	float distThreshold; //distance at which to apply the threshold
	float zmin, zmax;	//minimum and maximum distance to extrude the prism object
public:
	Segmentation(bool running_, ros::NodeHandle nh)
	: nh_(nh), updateCloud(false), save(false), running(running_), cloudName("Segmentation Cloud"),
	hardware_concurrency(std::thread::hardware_concurrency()),
	spinner(hardware_concurrency/2)
	{
	    cloud_sub_ = nh.subscribe("ensenso/cloud", 10, &Segmentation::cloudCallback, this);
		    normals = PointCloudNPtr(new PointCloudN());
	}

	~Segmentation()
	{
		// viewer->close();
	}

	void spawn()
	{
		begin();
		end();
	}

	void begin()
	{
		if(spinner.canStart())
		{
			spinner.start();
			ROS_INFO("started spinner");
		}

		while(!updateCloud)
		{
		  std::this_thread::sleep_for(std::chrono::milliseconds(1));
		}

		//spawn the threads
	    threads.push_back(std::thread(&Segmentation::planeSeg, this));
	    threads.push_back(std::thread(&Segmentation::headSeg, this));

	    std::for_each(threads.begin(), threads.end(), \
	                  std::mem_fn(&std::thread::join));

	    // cloudDispThread = std::thread(&Segmentation::cloudDisp, this);
	    normals = PointCloudNPtr (new PointCloudN());
	}

	void end()
	{
	  spinner.stop();
	  running = false;
	}

	void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& ensensoCloud)
	{
		PointCloudT cloud;

		getCloud(ensensoCloud, cloud);
		// getSurfaceNormals(cloud, this->normals);

		std::lock_guard<std::mutex> lock(mutex);
		this->cloud = cloud;
		updateCloud = true;
	}

	void getCloud(const sensor_msgs::PointCloud2ConstPtr cb_cloud, PointCloudT& pcl_cloud) const
	{
		pcl::PCLPointCloud2 pcl_pc;
		pcl_conversions::toPCL(*cb_cloud, pcl_pc);
		pcl::fromPCLPointCloud2(pcl_pc, pcl_cloud);
	}

	void cloudDisp()
	{
	  const PointCloudT& cloud  = this->cloud;
	  PointCloudT::ConstPtr cloud_ptr (&cloud);
	  PointCloudNPtr normals(new PointCloudN);
	  normals = this->normals;

	  pcl::visualization::PointCloudColorHandlerCustom<PointT> color_handler (cloud_ptr, 255, 255, 255);
	  viz = boost::shared_ptr<visualizer> (new visualizer());

	  viewer= viz->createViewer();
	  viewer->addPointCloud(cloud_ptr, color_handler, cloudName);
	  viewer->addPointCloudNormals<PointT, PointN>(cloud_ptr, normals, 20, 0.03, "normals");

	  for(; running && ros::ok() ;)
	  {
	    /*populate the cloud viewer and prepare for publishing*/
	    if(updateCloud)
	    {
	      std::lock_guard<std::mutex> lock(mutex);
	      updateCloud = false;
	      viewer->updatePointCloud(cloud_ptr, cloudName);
	      // viewer->addPointCloudNormals<PointT, PointN>(cloud_ptr, normals, 20, 0.03, "normals");
	    }
	    viewer->spinOnce(10);
	  }
	  viewer->close();
	}

	void getSurfaceNormals(const PointCloudT& cloud, const PointCloudNPtr normals)
	{
		PointCloudT::ConstPtr cloud_ptr (&cloud);
		normalEstimation.setInputCloud(cloud_ptr);
		normalEstimation.setRadiusSearch(0.03); //use all neighbors in a radius of 3cm.
		// use a kd-tree to search for neighbors.
		pcl::search::KdTree<PointT>::Ptr kdtree(new pcl::search::KdTree<PointT>);
		normalEstimation.setSearchMethod(kdtree);

		// Calculate the normals.
		normalEstimation.compute(*normals);
	}

	void planeSeg()
	{
		// Objects for storing the point clouds.
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr convexHull(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr objects(new pcl::PointCloud<pcl::PointXYZ>);

		//Get Plane Model
		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
		// Create the segmentation object
		pcl::SACSegmentation<PointT> seg;
		// Optional
		seg.setOptimizeCoefficients (true);
		/*Set the maximum number of iterations before giving up*/
		seg.setMaxIterations(100);
		// Mandatory
		seg.setModelType (pcl::SACMODEL_PLANE);
		seg.setMethodType (pcl::SAC_RANSAC);
		/*set the distance to the model threshold to use*/
		distThreshold = 0.712;
		seg.setDistanceThreshold (distThreshold);	//as measured
		{
			std::lock_guard<std::mutex> lock(mutex);
			*cloud = this->cloud;
			updateCloud = false;
		}

		seg.setInputCloud (cloud);
		seg.segment (*inliers, *coefficients);

		if(inliers->indices.size() == 0)
			ROS_INFO("Could not find plane in scene");
		else
		{
			// Copy the points of the plane to a new cloud.
			pcl::ExtractIndices<pcl::PointXYZ> extract;
			extract.setInputCloud(cloud);
			extract.setIndices(inliers);
			extract.filter(*plane);

			//retrieve convex hull
			pcl::ConvexHull<PointT> hull;
			hull.setInputCloud(plane);

			//ensure hull is 2 -d
			hull.setDimension(2);
			hull.reconstruct(*convexHull);

			//redundant check
			if (hull.getDimension() == 2)
			{
				// Prism object.
				pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
				prism.setInputCloud(cloud);
				prism.setInputPlanarHull(convexHull);
				// First parameter: minimum Z value. Set to 0, segments objects lying on the plane (can be negative).
				// Second parameter: maximum Z value, set to 10cm. Tune it according to the height of the objects you expect.
				// prism.setHeightLimits(0.1f, 0.5f);
				zmin = 0.2f; zmax = 0.4953f+0.2f;
				prism.setHeightLimits(zmin, zmax);
				pcl::PointIndices::Ptr objectIndices(new pcl::PointIndices);

				prism.segment(*objectIndices);

				// Get and show all points retrieved by the hull.
				extract.setIndices(objectIndices);
				extract.filter(*objects);

				multiViewer = boost::shared_ptr<pcl::visualization::PCLVisualizer> (new pcl::visualization::PCLVisualizer ("Multiple Viewer"));
				multiViewer->initCameraParameters ();

				int v1(0);
				multiViewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
				multiViewer->setBackgroundColor (0, 0, 0, v1);
				multiViewer->addText("Original Cloud: 0.01", 10, 10, "v1 text", v1);
				multiViewer->addPointCloud(cloud, "Original Cloud", v1);

				int v2(0);
				multiViewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
				multiViewer->setBackgroundColor (0.3, 0.3, 0.3, v2);
				multiViewer->addText("Segmented Cloud", 10, 10, "v2 text", v2);
				multiViewer->addPointCloud(objects, "Segmented Cloud", v2);

				multiViewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Original Cloud");
				multiViewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Segmented Cloud");

				while (running && ros::ok())
				{
					/*populate the cloud viewer and prepare for publishing*/
					if(updateCloud)
					{
					  std::lock_guard<std::mutex> lock(mutex);
					  updateCloud = false;
					  multiViewer->updatePointCloud(cloud, "Original Cloud");
					  multiViewer->updatePointCloud(objects, "Segmented Cloud");
					}
					multiViewer->spinOnce(10);
				}
				multiViewer->close();
			}
			else
				ROS_INFO("Chosen hull is not planar");
		}
	}

	void headSeg()
	{
		// Objects for storing the point clouds.
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr head(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr convexHull(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr objects(new pcl::PointCloud<pcl::PointXYZ>);

		//Get Plane Model
		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
		//set min and max radius for sphere discovery
		const double& min_radius = 0.01;
		const double& max_radius = 0.03;
		// Create the segmentation object
		pcl::SACSegmentation<PointT> seg;
		// Optional
		seg.setOptimizeCoefficients (true);
		// Mandatory
		seg.setModelType (pcl::SACMODEL_SPHERE);
		seg.setMethodType (pcl::SAC_RANSAC);
		seg.setRadiusLimits(min_radius, max_radius);
		// seg.setDistanceThreshold (0.01);
		//set axis along which to search for a model perpendicular to
		Eigen::Vector3f ax;
		ax << 0, 0, 1;
		seg.setAxis(ax);

		{
			// mutex.lock();
			std::lock_guard<std::mutex> lock(mutex);
			*cloud = this->cloud;
			updateCloud = false;
		}

		seg.setInputCloud (cloud);
		//inliers: resultant pt indices that support found model
		//coefficients: resultant model coefficients
		seg.segment (*inliers, *coefficients);

		if(inliers->indices.size() == 0)
			ROS_INFO("Could not find sphere in scene");
		else
		{
			// Copy the points of the spherepcl::extract indices to a new cloud.
			pcl::ExtractIndices<pcl::PointXYZ> extract(true); //init with true will always allow to extract removed indices
			extract.setInputCloud(cloud);
			extract.setIndices(inliers);
			//the resulting cloud that contains the indices of the head spherical model
			extract.filter(*head);

			//retrieve convex hull
			pcl::ConvexHull<PointT> hull;
			hull.setInputCloud(head);

			//ensure hull is 3 -d
			hull.setDimension(3);
			/*compute a convex hull for all points given*/
			hull.reconstruct(*convexHull);
			/*compute the area volume of the head and output the result
			to console*/
			hull.setComputeAreaVolume(true);

			//redundant check
			if (hull.getDimension() == 3)
			{
				// Prism object.
				pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
				prism.setInputCloud(cloud);
				prism.setInputPlanarHull(convexHull);
				// First parameter: minimum Z value. Set to 0, segments objects lying on the plane (can be negative).
				// Second parameter: maximum Z value, set to 50cm. Tune it according to the height of the objects you expect.
				prism.setHeightLimits(0.2f, 0.6f);
				pcl::PointIndices::Ptr objectIndices(new pcl::PointIndices);

				prism.segment(*objectIndices);

				// Get and show all points retrieved by the hull.
				extract.setIndices(objectIndices);
				extract.filter(*objects);

				multiViewer = boost::shared_ptr<pcl::visualization::PCLVisualizer> (new pcl::visualization::PCLVisualizer ("Multiple Viewer"));
				multiViewer->initCameraParameters ();

				int v1(0);
				multiViewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
				multiViewer->setBackgroundColor (0, 0, 0, v1);
				multiViewer->addText("Original Cloud: 0.01", 10, 10, "v1 text", v1);
				multiViewer->addPointCloud(cloud, "Original Cloud", v1);

				int v2(0);
				multiViewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
				multiViewer->setBackgroundColor (0.3, 0.3, 0.3, v2);
				multiViewer->addText("Segmented Cloud", 10, 10, "v2 text", v2);
				multiViewer->addPointCloud(objects, "Segmented Cloud", v2);

				multiViewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Original Cloud");
				multiViewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Segmented Cloud");

				while (running && ros::ok())
				{
					/*populate the cloud viewer and prepare for publishing*/
					if(updateCloud)
					{
					  std::lock_guard<std::mutex> lock(mutex);
					  updateCloud = false;
					  multiViewer->updatePointCloud(cloud, "Original Cloud");
					  multiViewer->updatePointCloud(objects, "Segmented Cloud");
					}
					multiViewer->spinOnce(10);
				}
				multiViewer->close();
			}
			else
				ROS_INFO("Chosen hull is not planar");
		}
	}
};

int main(int argc, char* argv[])
{
	ros::init(argc, argv, "ensensor_segmentation_node");

	ROS_INFO("Started node %s", ros::this_node::getName().c_str());

	bool running = true;
	ros::NodeHandle nh;

	Segmentation seg(running, nh);
	seg.spawn();

	ros::shutdown();
}<commit_msg>plane segmentation working<commit_after>#include <mutex>
#include <thread>
#include <ros/spinner.h>

#include <ensenso/visualizer.h>
#include <ensenso/pcl_headers.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>

/*Globlal namespaces and aliases*/
// using namespace pathfinder;
using PointT 			= pcl::PointXYZ;
using PointCloudT 		= pcl::PointCloud<PointT>;
using PointCloudTPtr 	= PointCloudT::Ptr;

using PointN  			= pcl::Normal;
using PointCloudN 		= pcl::PointCloud<PointN>;
using PointCloudNPtr  	= PointCloudN::Ptr;

using pcl_viz 			= pcl::visualization::PCLVisualizer;
using NormalEstimation 	= pcl::NormalEstimation<PointT, PointN>;

#define OUT(__o__) std::cout<< __o__ << std::endl;

class Segmentation
{
private:
	bool updateCloud, save, running;
	ros::NodeHandle nh_;
 	const std::string cloudName;
	ros::Subscriber cloud_sub_;
	std::mutex mutex;
 	PointCloudT cloud;
 	std::thread cloudDispThread, normalsDispThread;
 	unsigned long const hardware_concurrency;
 	ros::AsyncSpinner spinner;
 	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer, segViewer;
 	boost::shared_ptr<pcl::visualization::PCLVisualizer> multiViewer;
 	NormalEstimation normalEstimation;
	PointCloudNPtr normals;

	boost::shared_ptr<visualizer> viz;
	std::vector<std::thread> threads;
	float distThreshold; //distance at which to apply the threshold
	float zmin, zmax;	//minimum and maximum distance to extrude the prism object
public:
	Segmentation(bool running_, ros::NodeHandle nh)
	: nh_(nh), updateCloud(false), save(false), running(running_), cloudName("Segmentation Cloud"),
	hardware_concurrency(std::thread::hardware_concurrency()),
	spinner(hardware_concurrency/2)
	{
	    cloud_sub_ = nh.subscribe("ensenso/cloud", 10, &Segmentation::cloudCallback, this);
		    normals = PointCloudNPtr(new PointCloudN());
	}

	~Segmentation()
	{
		// viewer->close();
	}

	void spawn()
	{
		begin();
		end();
	}

	void begin()
	{
		if(spinner.canStart())
		{
			spinner.start();
			ROS_INFO("started spinner");
		}

		while(!updateCloud)
		{
		  std::this_thread::sleep_for(std::chrono::milliseconds(1));
		}

		//spawn the threads
	    threads.push_back(std::thread(&Segmentation::planeSeg, this));
	    threads.push_back(std::thread(&Segmentation::headSeg, this));

	    std::for_each(threads.begin(), threads.end(), \
	                  std::mem_fn(&std::thread::join));

	    // cloudDispThread = std::thread(&Segmentation::cloudDisp, this);
	    normals = PointCloudNPtr (new PointCloudN());
	}

	void end()
	{
	  spinner.stop();
	  running = false;
	}

	void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& ensensoCloud)
	{
		PointCloudT cloud;

		getCloud(ensensoCloud, cloud);
		// getSurfaceNormals(cloud, this->normals);

		std::lock_guard<std::mutex> lock(mutex);
		this->cloud = cloud;
		updateCloud = true;
	}

	void getCloud(const sensor_msgs::PointCloud2ConstPtr cb_cloud, PointCloudT& pcl_cloud) const
	{
		pcl::PCLPointCloud2 pcl_pc;
		pcl_conversions::toPCL(*cb_cloud, pcl_pc);
		pcl::fromPCLPointCloud2(pcl_pc, pcl_cloud);
	}

	void cloudDisp()
	{
	  const PointCloudT& cloud  = this->cloud;
	  PointCloudT::ConstPtr cloud_ptr (&cloud);
	  PointCloudNPtr normals(new PointCloudN);
	  normals = this->normals;

	  pcl::visualization::PointCloudColorHandlerCustom<PointT> color_handler (cloud_ptr, 255, 255, 255);
	  viz = boost::shared_ptr<visualizer> (new visualizer());

	  viewer= viz->createViewer();
	  viewer->addPointCloud(cloud_ptr, color_handler, cloudName);
	  viewer->addPointCloudNormals<PointT, PointN>(cloud_ptr, normals, 20, 0.03, "normals");

	  for(; running && ros::ok() ;)
	  {
	    /*populate the cloud viewer and prepare for publishing*/
	    if(updateCloud)
	    {
	      std::lock_guard<std::mutex> lock(mutex);
	      updateCloud = false;
	      viewer->updatePointCloud(cloud_ptr, cloudName);
	      // viewer->addPointCloudNormals<PointT, PointN>(cloud_ptr, normals, 20, 0.03, "normals");
	    }
	    viewer->spinOnce(10);
	  }
	  viewer->close();
	}

	void getSurfaceNormals(const PointCloudT& cloud, const PointCloudNPtr normals)
	{
		PointCloudT::ConstPtr cloud_ptr (&cloud);
		normalEstimation.setInputCloud(cloud_ptr);
		normalEstimation.setRadiusSearch(0.03); //use all neighbors in a radius of 3cm.
		// use a kd-tree to search for neighbors.
		pcl::search::KdTree<PointT>::Ptr kdtree(new pcl::search::KdTree<PointT>);
		normalEstimation.setSearchMethod(kdtree);

		// Calculate the normals.
		normalEstimation.compute(*normals);
	}

	void planeSeg()
	{
		// Objects for storing the point clouds.
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr convexHull(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr objects(new pcl::PointCloud<pcl::PointXYZ>);

		//Get Plane Model
		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
		// Create the segmentation object
		pcl::SACSegmentation<PointT> seg;
		// Optional
		seg.setOptimizeCoefficients (true);
		/*Set the maximum number of iterations before giving up*/
		seg.setMaxIterations(100);
		// Mandatory
		seg.setModelType (pcl::SACMODEL_PLANE);
		seg.setMethodType (pcl::SAC_RANSAC);
		/*set the distance to the model threshold to use*/
		distThreshold = 0.712;
		seg.setDistanceThreshold (distThreshold);	//as measured
		{
			std::lock_guard<std::mutex> lock(mutex);
			*cloud = this->cloud;
			updateCloud = false;
		}

		seg.setInputCloud (cloud);
		seg.segment (*inliers, *coefficients);

		if(inliers->indices.size() == 0)
			ROS_INFO("Could not find plane in scene");
		else
		{
			// Copy the points of the plane to a new cloud.
			pcl::ExtractIndices<pcl::PointXYZ> extract;
			extract.setInputCloud(cloud);
			extract.setIndices(inliers);
			extract.filter(*plane);

			//retrieve convex hull
			pcl::ConvexHull<PointT> hull;
			hull.setInputCloud(plane);

			//ensure hull is 2 -d
			hull.setDimension(2);
			hull.reconstruct(*convexHull);

			//redundant check
			if (hull.getDimension() == 2)
			{
				// Prism object.
				pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
				prism.setInputCloud(cloud);
				prism.setInputPlanarHull(convexHull);
				// First parameter: minimum Z value. Set to 0, segments objects lying on the plane (can be negative).
				// Second parameter: maximum Z value, set to 10cm. Tune it according to the height of the objects you expect.
				// prism.setHeightLimits(0.1f, 0.5f);
				zmin = 0.2f; zmax = 0.4953f+0.2f;
				prism.setHeightLimits(zmin, zmax);
				pcl::PointIndices::Ptr objectIndices(new pcl::PointIndices);

				prism.segment(*objectIndices);

				// Get and show all points retrieved by the hull.
				extract.setIndices(objectIndices);
				extract.filter(*objects);

				multiViewer = boost::shared_ptr<pcl::visualization::PCLVisualizer> (new pcl::visualization::PCLVisualizer ("Multiple Viewer"));
				multiViewer->initCameraParameters ();

				int v1(0);
				multiViewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
				multiViewer->setBackgroundColor (0, 0, 0, v1);
				multiViewer->addText("Original Cloud: 0.01", 10, 10, "v1 text", v1);
				multiViewer->addPointCloud(cloud, "Original Cloud", v1);

				int v2(0);
				multiViewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
				multiViewer->setBackgroundColor (0.3, 0.3, 0.3, v2);
				multiViewer->addText("Segmented Cloud", 10, 10, "v2 text", v2);
				multiViewer->addPointCloud(objects, "Segmented Cloud", v2);

				multiViewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Original Cloud");
				multiViewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Segmented Cloud");

				while (running && ros::ok())
				{
					/*populate the cloud viewer and prepare for publishing*/
					if(updateCloud)
					{
					  std::lock_guard<std::mutex> lock(mutex);
					  updateCloud = false;
					  multiViewer->updatePointCloud(cloud, "Original Cloud");
					  multiViewer->updatePointCloud(objects, "Segmented Cloud");
					}
					multiViewer->spinOnce(10);
				}
				multiViewer->close();
			}
			else
				ROS_INFO("Chosen hull is not planar");
		}
	}
};

int main(int argc, char* argv[])
{
	ros::init(argc, argv, "ensensor_segmentation_node");

	ROS_INFO("Started node %s", ros::this_node::getName().c_str());

	bool running = true;
	ros::NodeHandle nh;

	Segmentation seg(running, nh);
	seg.spawn();

	ros::shutdown();
}<|endoftext|>


<commit_before>/*
 * Copyright 2019
 * Joseph Mirabel
 *
 * LAAS-CNRS
 *
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* --- SOT --- */
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>

#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>

#include <pinocchio/multibody/liegroup/liegroup.hpp>

#include <Eigen/LU>

#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/feature-pose.hh>

using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;

typedef pinocchio::CartesianProductOperation<
    pinocchio::VectorSpaceOperationTpl<3, double>,
    pinocchio::SpecialOrthogonalOperationTpl<3, double> >
    R3xSO3_t;
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
template <Representation_t representation> struct LG_t {
  typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
                                    R3xSO3_t>::type type;
};

typedef FeaturePose<R3xSO3Representation> FeaturePose_t;
typedef FeaturePose<SE3Representation> FeaturePoseSE3_t;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose");
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3");

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

static const MatrixHomogeneous Id(MatrixHomogeneous::Identity());

template <Representation_t representation>
FeaturePose<representation>::FeaturePose(const string &pointName)
    : FeatureAbstract(pointName),
      oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"),
      jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"),
      oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"),
      jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"),
      jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"),
      jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb")

      ,
      faMfbDes(NULL,
               CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"),
      faNufafbDes(NULL,
                  CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes")

      ,
      faMfb(
          boost::bind(&FeaturePose<representation>::computefaMfb, this, _1, _2),
          oMja << jaMfa << oMjb << jbMfb,
          CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes"),
      q_faMfb(boost::bind(&FeaturePose<representation>::computeQfaMfb, this, _1,
                          _2),
              faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"),
      q_faMfbDes(boost::bind(&FeaturePose<representation>::computeQfaMfbDes,
                             this, _1, _2),
                 faMfbDes,
                 CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") {
  oMja.setConstant(Id);
  jaMfa.setConstant(Id);
  jbMfb.setConstant(Id);
  faMfbDes.setConstant(Id);
  faNufafbDes.setConstant(Vector::Zero(6));

  jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb);

  errorSOUT.addDependencies(q_faMfbDes << q_faMfb);

  signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb);
  signalRegistration(errordotSOUT << faMfbDes << faNufafbDes);

  errordotSOUT.setFunction(
      boost::bind(&FeaturePose<representation>::computeErrorDot, this, _1, _2));
  errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes);

  // Commands
  //
  {
    using namespace dynamicgraph::command;
    addCommand("keep",
               makeCommandVoid1(
                   *this, &FeaturePose<representation>::servoCurrentPosition,
                   docCommandVoid1(
                       "modify the desired position to servo at current pos.",
                       "time")));
  }
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

template <Representation_t representation>
static inline void check(const FeaturePose<representation> &ft) {
  (void)ft;
  assert(ft.oMja.isPlugged());
  assert(ft.jaMfa.isPlugged());
  assert(ft.oMjb.isPlugged());
  assert(ft.jbMfb.isPlugged());
  assert(ft.faMfbDes.isPlugged());
  assert(ft.faNufafbDes.isPlugged());
}

template <Representation_t representation>
unsigned int &FeaturePose<representation>::getDimension(unsigned int &dim,
                                                        int time) {
  sotDEBUG(25) << "# In {" << endl;

  const Flags &fl = selectionSIN.access(time);

  dim = 0;
  for (int i = 0; i < 6; ++i)
    if (fl(i))
      dim++;

  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}

void toVector(const MatrixHomogeneous &M, Vector7 &v) {
  v.head<3>() = M.translation();
  QuaternionMap(v.tail<4>().data()) = M.linear();
}

Vector7 toVector(const MatrixHomogeneous &M) {
  Vector7 ret;
  toVector(M, ret);
  return ret;
}

template <Representation_t representation>
Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
  typedef typename LG_t<representation>::type LieGroup_t;

  check(*this);

  q_faMfb.recompute(time);
  q_faMfbDes.recompute(time);

  const int &dim = dimensionSOUT(time);
  const Flags &fl = selectionSIN(time);

  const Matrix &_jbJjb = jbJjb(time);

  const MatrixHomogeneous &_jbMfb =
      (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);

  const Matrix::Index cJ = _jbJjb.cols();
  J.resize(dim, cJ);

  MatrixTwist X;
  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;

  buildFrom(_jbMfb.inverse(Eigen::Affine), X);
  MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() *
                         oMja.access(time).rotation().transpose() *
                         oMjb.access(time).rotation() * _jbMfb.rotation();
  if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
    X.topRows<3>().applyOnTheLeft(faRfb);
  LieGroup_t().template dDifference<pinocchio::ARG1>(
      q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);

  // Contribution of b:
  // J = Jminus * X * jbJjb;
  unsigned int rJ = 0;
  for (unsigned int r = 0; r < 6; ++r)
    if (fl(r))
      J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;

  if (jaJja.isPlugged()) {
    const Matrix &_jaJja = jaJja(time);
    const MatrixHomogeneous &_jaMfa =
                                (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
                            _faMfb = faMfb.accessCopy();

    buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X);
    if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
      X.topRows<3>().applyOnTheLeft(faRfb);

    // J -= (Jminus * X) * jaJja(time);
    rJ = 0;
    for (unsigned int r = 0; r < 6; ++r)
      if (fl(r))
        J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
  }

  return J;
}

template <Representation_t representation>
MatrixHomogeneous &
FeaturePose<representation>::computefaMfb(MatrixHomogeneous &res, int time) {
  check(*this);

  res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) *
        jbMfb(time);
  return res;
}

template <Representation_t representation>
Vector7 &FeaturePose<representation>::computeQfaMfb(Vector7 &res, int time) {
  check(*this);

  toVector(faMfb(time), res);
  return res;
}

template <Representation_t representation>
Vector7 &FeaturePose<representation>::computeQfaMfbDes(Vector7 &res, int time) {
  check(*this);

  toVector(faMfbDes(time), res);
  return res;
}

template <Representation_t representation>
Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
  typedef typename LG_t<representation>::type LieGroup_t;
  check(*this);

  const Flags &fl = selectionSIN(time);

  Eigen::Matrix<double, 6, 1> v;
  LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v);

  error.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 6; ++i)
    if (fl(i))
      error(cursor++) = v(i);

  return error;
}

// This function is responsible of converting the input velocity expressed with
// SE(3) convention onto a velocity expressed with the convention of this
// feature (R^3xSO(3) or SE(3)), in the right frame.
template <typename LG_t>
Vector6d convertVelocity(const MatrixHomogeneous &M,
                         const MatrixHomogeneous &Mdes,
                         const Vector &faNufafbDes) {
  (void)M;
  MatrixTwist X;
  buildFrom(Mdes.inverse(Eigen::Affine), X);
  return X * faNufafbDes;
}
template <>
Vector6d convertVelocity<R3xSO3_t>(const MatrixHomogeneous &M,
                                   const MatrixHomogeneous &Mdes,
                                   const Vector &faNufafbDes) {
  Vector6d nu;
  nu.head<3>() =
      faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>());
  nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>();
  return nu;
}

template <Representation_t representation>
Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
                                                     int time) {
  typedef typename LG_t<representation>::type LieGroup_t;
  check(*this);

  errordot.resize(dimensionSOUT(time));
  const Flags &fl = selectionSIN(time);
  if (!faNufafbDes.isPlugged()) {
    errordot.setZero();
    return errordot;
  }

  q_faMfb.recompute(time);
  q_faMfbDes.recompute(time);
  faNufafbDes.recompute(time);

  const MatrixHomogeneous &_faMfbDes = faMfbDes(time);

  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;

  LieGroup_t().template dDifference<pinocchio::ARG0>(
      q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
  Vector6d nu = convertVelocity<LieGroup_t>(faMfb(time), _faMfbDes,
                                            faNufafbDes.accessCopy());
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 6; ++i)
    if (fl(i))
      errordot(cursor++) = Jminus.row(i) * nu;

  return errordot;
}

/* Modify the value of the reference (sdes) so that it corresponds
 * to the current position. The effect on the servo is to maintain the
 * current position and correct any drift. */
template <Representation_t representation>
void FeaturePose<representation>::servoCurrentPosition(const int &time) {
  check(*this);

  const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
                          _jaMfa =
                              (jaMfa.isPlugged() ? jaMfa.access(time) : Id),
                          _oMjb = oMjb.access(time),
                          _jbMfb =
                              (jbMfb.isPlugged() ? jbMfb.access(time) : Id);
  faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
}

static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
template <Representation_t representation>
void FeaturePose<representation>::display(std::ostream &os) const {
  os << CLASS_NAME << "<" << name << ">: (";

  try {
    const Flags &fl = selectionSIN.accessCopy();
    bool first = true;
    for (int i = 0; i < 6; ++i)
      if (fl(i)) {
        if (first) {
          first = false;
        } else {
          os << ",";
        }
        os << featureNames[i];
      }
    os << ") ";
  } catch (ExceptionAbstract e) {
    os << " selectSIN not set.";
  }
}
<commit_msg>[FeaturePose] Fix signal name<commit_after>/*
 * Copyright 2019
 * Joseph Mirabel
 *
 * LAAS-CNRS
 *
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* --- SOT --- */
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>

#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>

#include <pinocchio/multibody/liegroup/liegroup.hpp>

#include <Eigen/LU>

#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/feature-pose.hh>

using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;

typedef pinocchio::CartesianProductOperation<
    pinocchio::VectorSpaceOperationTpl<3, double>,
    pinocchio::SpecialOrthogonalOperationTpl<3, double> >
    R3xSO3_t;
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
template <Representation_t representation> struct LG_t {
  typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
                                    R3xSO3_t>::type type;
};

typedef FeaturePose<R3xSO3Representation> FeaturePose_t;
typedef FeaturePose<SE3Representation> FeaturePoseSE3_t;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose");
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3");

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

static const MatrixHomogeneous Id(MatrixHomogeneous::Identity());

template <Representation_t representation>
FeaturePose<representation>::FeaturePose(const string &pointName)
    : FeatureAbstract(pointName),
      oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"),
      jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"),
      oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"),
      jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"),
      jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"),
      jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb")

      ,
      faMfbDes(NULL,
               CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"),
      faNufafbDes(NULL,
                  CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes")

      ,
      faMfb(
          boost::bind(&FeaturePose<representation>::computefaMfb, this, _1, _2),
          oMja << jaMfa << oMjb << jbMfb,
          CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"),
      q_faMfb(boost::bind(&FeaturePose<representation>::computeQfaMfb, this, _1,
                          _2),
              faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"),
      q_faMfbDes(boost::bind(&FeaturePose<representation>::computeQfaMfbDes,
                             this, _1, _2),
                 faMfbDes,
                 CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") {
  oMja.setConstant(Id);
  jaMfa.setConstant(Id);
  jbMfb.setConstant(Id);
  faMfbDes.setConstant(Id);
  faNufafbDes.setConstant(Vector::Zero(6));

  jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb);

  errorSOUT.addDependencies(q_faMfbDes << q_faMfb);

  signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb);
  signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes);

  errordotSOUT.setFunction(
      boost::bind(&FeaturePose<representation>::computeErrorDot, this, _1, _2));
  errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes);

  // Commands
  //
  {
    using namespace dynamicgraph::command;
    addCommand("keep",
               makeCommandVoid1(
                   *this, &FeaturePose<representation>::servoCurrentPosition,
                   docCommandVoid1(
                       "modify the desired position to servo at current pos.",
                       "time")));
  }
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

template <Representation_t representation>
static inline void check(const FeaturePose<representation> &ft) {
  (void)ft;
  assert(ft.oMja.isPlugged());
  assert(ft.jaMfa.isPlugged());
  assert(ft.oMjb.isPlugged());
  assert(ft.jbMfb.isPlugged());
  assert(ft.faMfbDes.isPlugged());
  assert(ft.faNufafbDes.isPlugged());
}

template <Representation_t representation>
unsigned int &FeaturePose<representation>::getDimension(unsigned int &dim,
                                                        int time) {
  sotDEBUG(25) << "# In {" << endl;

  const Flags &fl = selectionSIN.access(time);

  dim = 0;
  for (int i = 0; i < 6; ++i)
    if (fl(i))
      dim++;

  sotDEBUG(25) << "# Out }" << endl;
  return dim;
}

void toVector(const MatrixHomogeneous &M, Vector7 &v) {
  v.head<3>() = M.translation();
  QuaternionMap(v.tail<4>().data()) = M.linear();
}

Vector7 toVector(const MatrixHomogeneous &M) {
  Vector7 ret;
  toVector(M, ret);
  return ret;
}

template <Representation_t representation>
Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
  typedef typename LG_t<representation>::type LieGroup_t;

  check(*this);

  q_faMfb.recompute(time);
  q_faMfbDes.recompute(time);

  const int &dim = dimensionSOUT(time);
  const Flags &fl = selectionSIN(time);

  const Matrix &_jbJjb = jbJjb(time);

  const MatrixHomogeneous &_jbMfb =
      (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);

  const Matrix::Index cJ = _jbJjb.cols();
  J.resize(dim, cJ);

  MatrixTwist X;
  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;

  buildFrom(_jbMfb.inverse(Eigen::Affine), X);
  MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() *
                         oMja.access(time).rotation().transpose() *
                         oMjb.access(time).rotation() * _jbMfb.rotation();
  if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
    X.topRows<3>().applyOnTheLeft(faRfb);
  LieGroup_t().template dDifference<pinocchio::ARG1>(
      q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);

  // Contribution of b:
  // J = Jminus * X * jbJjb;
  unsigned int rJ = 0;
  for (unsigned int r = 0; r < 6; ++r)
    if (fl(r))
      J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;

  if (jaJja.isPlugged()) {
    const Matrix &_jaJja = jaJja(time);
    const MatrixHomogeneous &_jaMfa =
                                (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
                            _faMfb = faMfb.accessCopy();

    buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X);
    if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
      X.topRows<3>().applyOnTheLeft(faRfb);

    // J -= (Jminus * X) * jaJja(time);
    rJ = 0;
    for (unsigned int r = 0; r < 6; ++r)
      if (fl(r))
        J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
  }

  return J;
}

template <Representation_t representation>
MatrixHomogeneous &
FeaturePose<representation>::computefaMfb(MatrixHomogeneous &res, int time) {
  check(*this);

  res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) *
        jbMfb(time);
  return res;
}

template <Representation_t representation>
Vector7 &FeaturePose<representation>::computeQfaMfb(Vector7 &res, int time) {
  check(*this);

  toVector(faMfb(time), res);
  return res;
}

template <Representation_t representation>
Vector7 &FeaturePose<representation>::computeQfaMfbDes(Vector7 &res, int time) {
  check(*this);

  toVector(faMfbDes(time), res);
  return res;
}

template <Representation_t representation>
Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
  typedef typename LG_t<representation>::type LieGroup_t;
  check(*this);

  const Flags &fl = selectionSIN(time);

  Eigen::Matrix<double, 6, 1> v;
  LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v);

  error.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 6; ++i)
    if (fl(i))
      error(cursor++) = v(i);

  return error;
}

// This function is responsible of converting the input velocity expressed with
// SE(3) convention onto a velocity expressed with the convention of this
// feature (R^3xSO(3) or SE(3)), in the right frame.
template <typename LG_t>
Vector6d convertVelocity(const MatrixHomogeneous &M,
                         const MatrixHomogeneous &Mdes,
                         const Vector &faNufafbDes) {
  (void)M;
  MatrixTwist X;
  buildFrom(Mdes.inverse(Eigen::Affine), X);
  return X * faNufafbDes;
}
template <>
Vector6d convertVelocity<R3xSO3_t>(const MatrixHomogeneous &M,
                                   const MatrixHomogeneous &Mdes,
                                   const Vector &faNufafbDes) {
  Vector6d nu;
  nu.head<3>() =
      faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>());
  nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>();
  return nu;
}

template <Representation_t representation>
Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
                                                     int time) {
  typedef typename LG_t<representation>::type LieGroup_t;
  check(*this);

  errordot.resize(dimensionSOUT(time));
  const Flags &fl = selectionSIN(time);
  if (!faNufafbDes.isPlugged()) {
    errordot.setZero();
    return errordot;
  }

  q_faMfb.recompute(time);
  q_faMfbDes.recompute(time);
  faNufafbDes.recompute(time);

  const MatrixHomogeneous &_faMfbDes = faMfbDes(time);

  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;

  LieGroup_t().template dDifference<pinocchio::ARG0>(
      q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
  Vector6d nu = convertVelocity<LieGroup_t>(faMfb(time), _faMfbDes,
                                            faNufafbDes.accessCopy());
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 6; ++i)
    if (fl(i))
      errordot(cursor++) = Jminus.row(i) * nu;

  return errordot;
}

/* Modify the value of the reference (sdes) so that it corresponds
 * to the current position. The effect on the servo is to maintain the
 * current position and correct any drift. */
template <Representation_t representation>
void FeaturePose<representation>::servoCurrentPosition(const int &time) {
  check(*this);

  const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
                          _jaMfa =
                              (jaMfa.isPlugged() ? jaMfa.access(time) : Id),
                          _oMjb = oMjb.access(time),
                          _jbMfb =
                              (jbMfb.isPlugged() ? jbMfb.access(time) : Id);
  faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
}

static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
template <Representation_t representation>
void FeaturePose<representation>::display(std::ostream &os) const {
  os << CLASS_NAME << "<" << name << ">: (";

  try {
    const Flags &fl = selectionSIN.accessCopy();
    bool first = true;
    for (int i = 0; i < 6; ++i)
      if (fl(i)) {
        if (first) {
          first = false;
        } else {
          os << ",";
        }
        os << featureNames[i];
      }
    os << ") ";
  } catch (ExceptionAbstract e) {
    os << " selectSIN not set.";
  }
}
<|endoftext|>


<commit_before>// Copyright 2017 Massachusetts Institute of Technology

#include <csignal>

#include "ros/ros.h"
#include "nodelet/nodelet.h"

#include "global_mapper_ros/global_mapper_ros.h"

namespace global_mapper {

class GlobalMapperNodelet : public nodelet::Nodelet {
 public:
  GlobalMapperNodelet() = default;
  ~GlobalMapperNodelet() = default;

  GlobalMapperNodelet(const GlobalMapperNodelet& rhs) = delete;
  GlobalMapperNodelet& operator=(const GlobalMapperNodelet& rhs) = delete;

  GlobalMapperNodelet(GlobalMapperNodelet&& rhs) = delete;
  GlobalMapperNodelet& operator=(GlobalMapperNodelet&& rhs) = delete;

  /**
   * We override the default ROS SIGINT handler to set a global variable which
   * tells the other threads to stop.
   */
  static void signal_handler(int signal) {
    printf("(svis_ros) SIGINT received\n");

    // Tell other threads to stop.
    GlobalMapperNodelet::stop_signal_ = 1;

    // Tell ROS to shutdown nodes.
    ros::shutdown();

    return;
  }

  // sigint
  static volatile std::sig_atomic_t stop_signal_;

  /**
   * \brief Nodelet initialization.
   *
   * Subclasses of nodelet::Nodelet need to override this virtual method.
   * It takes the place of the nodelet constructor.
   */
  virtual void onInit() {
    // Install signal handler.
    std::signal(SIGINT, signal_handler);

    // GlobalMapperRos global_mapper_ros;
    // NODELET_INFO("Starting loop");
    // global_mapper_ros.Run(stop_signal_);

    return;
  }
};

volatile std::sig_atomic_t stop_signal_ = 0;

}  // namespace global_mapper

// Export as a plugin.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(global_mapper::GlobalMapperNodelet, nodelet::Nodelet)
<commit_msg>Fix sigint implementation.<commit_after>// Copyright 2017 Massachusetts Institute of Technology

#include <csignal>

#include "ros/ros.h"
#include "nodelet/nodelet.h"

#include "global_mapper_ros/global_mapper_ros.h"

namespace global_mapper {

class GlobalMapperNodelet : public nodelet::Nodelet {
 public:
  GlobalMapperNodelet() = default;
  ~GlobalMapperNodelet() = default;

  GlobalMapperNodelet(const GlobalMapperNodelet& rhs) = delete;
  GlobalMapperNodelet& operator=(const GlobalMapperNodelet& rhs) = delete;

  GlobalMapperNodelet(GlobalMapperNodelet&& rhs) = delete;
  GlobalMapperNodelet& operator=(GlobalMapperNodelet&& rhs) = delete;

  /**
   * We override the default ROS SIGINT handler to set a global variable which
   * tells the other threads to stop.
   */
  static void signal_handler(int signal) {
    printf("(global_mapper) SIGINT received\n");

    // Tell other threads to stop.
    GlobalMapperNodelet::stop_signal_ = 1;

    // Tell ROS to shutdown nodes.
    ros::shutdown();

    return;
  }

  /**
   * \brief Nodelet initialization.
   *
   * Subclasses of nodelet::Nodelet need to override this virtual method.
   * It takes the place of the nodelet constructor.
   */
  virtual void onInit() {
    // Install signal handler.
    std::signal(SIGINT, signal_handler);

    // GlobalMapperRos global_mapper_ros;
    // NODELET_INFO("Starting loop");
    // global_mapper_ros.Run(stop_signal_);

    return;
  }

  // sigint
  static volatile std::sig_atomic_t stop_signal_;
};

volatile std::sig_atomic_t GlobalMapperNodelet::stop_signal_ = 0;

}  // namespace global_mapper

// Export as a plugin.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(global_mapper::GlobalMapperNodelet, nodelet::Nodelet)
<|endoftext|>


<commit_before>///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (c) 2019, The Regents of the University of California
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// * Neither the name of the copyright holder 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 HOLDER 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.

#include "scriptWidget.h"

#include <errno.h>
#include <unistd.h>

#include <QCoreApplication>
#include <QHBoxLayout>
#include <QKeyEvent>
#include <QTimer>
#include <QVBoxLayout>

#include "ord/OpenRoad.hh"
#include "utl/Logger.h"
#include "spdlog/formatter.h"
#include "spdlog/sinks/base_sink.h"

namespace gui {

ScriptWidget::ScriptWidget(QWidget* parent)
    : QDockWidget("Scripting", parent),
      output_(new QTextEdit),
      input_(new TclCmdInputWidget),
      pauser_(new QPushButton("Idle")),
      historyPosition_(0)
{
  setObjectName("scripting");  // for settings

  output_->setReadOnly(true);
  pauser_->setEnabled(false);
  pauser_->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Expanding);

  QHBoxLayout* inner_layout = new QHBoxLayout;
  inner_layout->addWidget(pauser_);
  inner_layout->addWidget(input_);

  QVBoxLayout* layout = new QVBoxLayout;
  layout->addWidget(output_, /* stretch */ 1);
  layout->addLayout(inner_layout);

  QWidget* container = new QWidget;
  container->setLayout(layout);

  QTimer::singleShot(200, this, &ScriptWidget::setupTcl);

  connect(input_, SIGNAL(completeCommand()), this, SLOT(executeCommand()));
  connect(this, SIGNAL(commandExecuted(int)), input_, SLOT(commandExecuted(int)));
  connect(input_, SIGNAL(historyGoBack()), this, SLOT(goBackHistory()));
  connect(input_, SIGNAL(historyGoForward()), this, SLOT(goForwardHistory()));
  connect(input_, SIGNAL(textChanged()), this, SLOT(outputChanged()));
  connect(output_, SIGNAL(textChanged()), this, SLOT(outputChanged()));
  connect(pauser_, SIGNAL(pressed()), this, SLOT(pauserClicked()));

  setWidget(container);
}

int channelClose(ClientData instance_data, Tcl_Interp* interp)
{
  // This channel should never be closed
  return EINVAL;
}

int ScriptWidget::channelOutput(ClientData instance_data,
                                const char* buf,
                                int to_write,
                                int* error_code)
{
  // Buffer up the output
  ScriptWidget* widget = (ScriptWidget*) instance_data;
  widget->logger_->report(std::string(buf, to_write));
  return to_write;
}

void channelWatch(ClientData instance_data, int mask)
{
  // watch is not supported inside OpenROAD GUI
}

Tcl_ChannelType ScriptWidget::stdout_channel_type_ = {
    // Tcl stupidly defines this a non-cost char*
    ((char*) "stdout_channel"),  /* typeName */
    TCL_CHANNEL_VERSION_2,       /* version */
    channelClose,                /* closeProc */
    nullptr,                     /* inputProc */
    ScriptWidget::channelOutput, /* outputProc */
    nullptr,                     /* seekProc */
    nullptr,                     /* setOptionProc */
    nullptr,                     /* getOptionProc */
    channelWatch,                /* watchProc */
    nullptr,                     /* getHandleProc */
    nullptr,                     /* close2Proc */
    nullptr,                     /* blockModeProc */
    nullptr,                     /* flushProc */
    nullptr,                     /* handlerProc */
    nullptr,                     /* wideSeekProc */
    nullptr,                     /* threadActionProc */
    nullptr                      /* truncateProc */
};

int ScriptWidget::tclExitHandler(ClientData instance_data,
                                 Tcl_Interp *interp,
                                 int argc,
                                 const char **argv) {
  ScriptWidget* widget = (ScriptWidget*) instance_data;
  // announces exit to Qt
  emit widget->tclExiting();
  // does not matter from here on, since GUI is getting ready exit
  return TCL_OK;
}

void ScriptWidget::setupTcl()
{
  interp_ = Tcl_CreateInterp();

  Tcl_Channel stdout_channel = Tcl_CreateChannel(
      &stdout_channel_type_, "stdout", (ClientData) this, TCL_WRITABLE);
  if (stdout_channel) {
    Tcl_SetChannelOption(nullptr, stdout_channel, "-translation", "lf");
    Tcl_SetChannelOption(nullptr, stdout_channel, "-buffering", "none");
    Tcl_RegisterChannel(interp_, stdout_channel);  // per man page: some tcl bug
    Tcl_SetStdChannel(stdout_channel, TCL_STDOUT);
  }

  // Overwrite exit to allow Qt to handle exit
  Tcl_CreateCommand(interp_, "exit", ScriptWidget::tclExitHandler, this, nullptr);

  // Ensures no newlines are present in stdout stream when using logger, but normal behavior in file writing
  Tcl_Eval(interp_, "rename puts ::tcl::openroad::puts");
  Tcl_Eval(interp_, "proc puts { args } { if {[llength $args] == 1} { ::tcl::openroad::puts -nonewline {*}$args } else { ::tcl::openroad::puts {*}$args } }");

  pauser_->setText("Running");
  pauser_->setStyleSheet("background-color: red");
  ord::tclAppInit(interp_);
  pauser_->setText("Idle");
  pauser_->setStyleSheet("");

  // TODO: tclAppInit should return the status which we could
  // pass to updateOutput
  addTclResultToOutput(TCL_OK);

  input_->init(interp_);
}

void ScriptWidget::executeCommand()
{
  pauser_->setText("Running");
  pauser_->setStyleSheet("background-color: red");
  QString command = input_->text();

  // Show the command that we executed
  addCommandToOutput(command);

  int return_code = Tcl_Eval(interp_, command.toLatin1().data());

  // Show its output
  addTclResultToOutput(return_code);

  if (return_code == TCL_OK) {
    // Update history; ignore repeated commands and keep last 100
    const int history_limit = 100;
    if (history_.empty() || command != history_.last()) {
      if (history_.size() == history_limit) {
        history_.pop_front();
      }

      history_.append(command);
    }
    historyPosition_ = history_.size();
  }

  pauser_->setText("Idle");
  pauser_->setStyleSheet("");

  emit commandExecuted(return_code);
}

void ScriptWidget::addCommandToOutput(const QString& cmd)
{
  const QString first_line_prefix    = ">>> ";
  const QString continue_line_prefix = "... ";

  QString command = first_line_prefix + cmd;
  command.replace("\n", "\n" + continue_line_prefix);

  addToOutput(command, cmd_msg_);
}

void ScriptWidget::addTclResultToOutput(int return_code)
{
  // Show the return value color-coded by ok/err.
  const char* result = Tcl_GetString(Tcl_GetObjResult(interp_));
  if (result[0] != '\0') {
    addToOutput(result, (return_code == TCL_OK) ? tcl_ok_msg_ : tcl_error_msg_);
  }
}

void ScriptWidget::addLogToOutput(const QString& text, const QColor& color)
{
  addToOutput(text, color);
}

void ScriptWidget::addReportToOutput(const QString& text)
{
  addToOutput(text, buffer_msg_);
}

void ScriptWidget::addToOutput(const QString& text, const QColor& color)
{
  // disable updates while added text
  output_->setUpdatesEnabled(false);

  // make sure cursor is at the end of the document
  output_->moveCursor(QTextCursor::End);

  QString output_text = text;
  if (text.endsWith('\n')) {
    // remove last new line
    output_text.chop(1);
  }

  // set new text color
  output_->setTextColor(color);

  QStringList output;
  for (QString& text_line : output_text.split('\n')) {
    // check for line length limits
    if (text_line.size() > max_output_line_length_) {
      text_line = text_line.left(max_output_line_length_-3);
      text_line += "...";
    }

    output.append(text_line);
  }
  // output new text
  output_->append(output.join("\n"));

  // reenable updates to display text
  output_->setUpdatesEnabled(true);

  // ensure changes are updated
  QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
}

ScriptWidget::~ScriptWidget()
{
  // TODO: I am being lazy and not cleaning up the tcl interpreter.
  // We are likely exiting anyways
}

void ScriptWidget::goForwardHistory()
{
  if (historyPosition_ < history_.size() - 1) {
    ++historyPosition_;
    input_->setText(history_[historyPosition_]);
  } else if (historyPosition_ == history_.size() - 1) {
    ++historyPosition_;
    input_->setText(history_buffer_last_);
  }
}

void ScriptWidget::goBackHistory()
{
  if (historyPosition_ > 0) {
    if (historyPosition_ == history_.size()) {
      // whats in the buffer is the last thing the user was editing
      history_buffer_last_ = input_->text();
    }
    --historyPosition_;
    input_->setText(history_[historyPosition_]);
  }
}

void ScriptWidget::readSettings(QSettings* settings)
{
  settings->beginGroup("scripting");
  history_ = settings->value("history").toStringList();
  historyPosition_ = history_.size();

  input_->readSettings(settings);

  settings->endGroup();
}

void ScriptWidget::writeSettings(QSettings* settings)
{
  settings->beginGroup("scripting");
  settings->setValue("history", history_);

  input_->writeSettings(settings);

  settings->endGroup();
}

void ScriptWidget::pause()
{
  QString prior_text = pauser_->text();
  bool prior_enable = pauser_->isEnabled();
  QString prior_style = pauser_->styleSheet();
  pauser_->setText("Continue");
  pauser_->setStyleSheet("background-color: yellow");
  pauser_->setEnabled(true);
  paused_ = true;

  // Keep processing events until the user continues
  while (paused_) {
    QCoreApplication::processEvents(QEventLoop::WaitForMoreEvents);
  }

  pauser_->setText(prior_text);
  pauser_->setStyleSheet(prior_style);
  pauser_->setEnabled(prior_enable);

  // Make changes visible while command runs
  QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
}

void ScriptWidget::pauserClicked()
{
  paused_ = false;
}

void ScriptWidget::outputChanged()
{
  // ensure the new output is visible
  output_->ensureCursorVisible();
  // Make changes visible
  QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
}

void ScriptWidget::resizeEvent(QResizeEvent* event)
{
  input_->setMaximumHeight(event->size().height() - output_->sizeHint().height());
  QDockWidget::resizeEvent(event);
}

void ScriptWidget::setFont(const QFont& font)
{
  QDockWidget::setFont(font);
  output_->setFont(font);
  input_->setFont(font);
}

// This class is an spdlog sink that writes the messages into the output
// area.
template <typename Mutex>
class ScriptWidget::GuiSink : public spdlog::sinks::base_sink<Mutex>
{
 public:
  GuiSink(ScriptWidget* widget) : widget_(widget) {}

 protected:
  void sink_it_(const spdlog::details::log_msg& msg) override
  {
    // Convert the msg into a formatted string
    spdlog::memory_buf_t formatted;
    this->formatter_->format(msg, formatted);
    std::string formatted_msg = std::string(formatted.data(), formatted.size());

    if (msg.level == spdlog::level::level_enum::off) {
      // this comes from a ->report
      widget_->addReportToOutput(formatted_msg.c_str());
    }
    else {
      // select error message color if message level is error or above.
      const QColor& msg_color = msg.level >= spdlog::level::level_enum::err ? widget_->tcl_error_msg_ : widget_->buffer_msg_;

      widget_->addLogToOutput(formatted_msg.c_str(), msg_color);
    }
  }

  void flush_() override {}

 private:
  ScriptWidget* widget_;
};

void ScriptWidget::setLogger(utl::Logger* logger)
{
  logger_ = logger;
  logger->addSink(std::make_shared<GuiSink<std::mutex>>(this));
}

}  // namespace gui
<commit_msg>gui: removing leftover updateEnabled toggles<commit_after>///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (c) 2019, The Regents of the University of California
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// * Neither the name of the copyright holder 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 HOLDER 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.

#include "scriptWidget.h"

#include <errno.h>
#include <unistd.h>

#include <QCoreApplication>
#include <QHBoxLayout>
#include <QKeyEvent>
#include <QTimer>
#include <QVBoxLayout>

#include "ord/OpenRoad.hh"
#include "utl/Logger.h"
#include "spdlog/formatter.h"
#include "spdlog/sinks/base_sink.h"

namespace gui {

ScriptWidget::ScriptWidget(QWidget* parent)
    : QDockWidget("Scripting", parent),
      output_(new QTextEdit),
      input_(new TclCmdInputWidget),
      pauser_(new QPushButton("Idle")),
      historyPosition_(0)
{
  setObjectName("scripting");  // for settings

  output_->setReadOnly(true);
  pauser_->setEnabled(false);
  pauser_->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Expanding);

  QHBoxLayout* inner_layout = new QHBoxLayout;
  inner_layout->addWidget(pauser_);
  inner_layout->addWidget(input_);

  QVBoxLayout* layout = new QVBoxLayout;
  layout->addWidget(output_, /* stretch */ 1);
  layout->addLayout(inner_layout);

  QWidget* container = new QWidget;
  container->setLayout(layout);

  QTimer::singleShot(200, this, &ScriptWidget::setupTcl);

  connect(input_, SIGNAL(completeCommand()), this, SLOT(executeCommand()));
  connect(this, SIGNAL(commandExecuted(int)), input_, SLOT(commandExecuted(int)));
  connect(input_, SIGNAL(historyGoBack()), this, SLOT(goBackHistory()));
  connect(input_, SIGNAL(historyGoForward()), this, SLOT(goForwardHistory()));
  connect(input_, SIGNAL(textChanged()), this, SLOT(outputChanged()));
  connect(output_, SIGNAL(textChanged()), this, SLOT(outputChanged()));
  connect(pauser_, SIGNAL(pressed()), this, SLOT(pauserClicked()));

  setWidget(container);
}

int channelClose(ClientData instance_data, Tcl_Interp* interp)
{
  // This channel should never be closed
  return EINVAL;
}

int ScriptWidget::channelOutput(ClientData instance_data,
                                const char* buf,
                                int to_write,
                                int* error_code)
{
  // Buffer up the output
  ScriptWidget* widget = (ScriptWidget*) instance_data;
  widget->logger_->report(std::string(buf, to_write));
  return to_write;
}

void channelWatch(ClientData instance_data, int mask)
{
  // watch is not supported inside OpenROAD GUI
}

Tcl_ChannelType ScriptWidget::stdout_channel_type_ = {
    // Tcl stupidly defines this a non-cost char*
    ((char*) "stdout_channel"),  /* typeName */
    TCL_CHANNEL_VERSION_2,       /* version */
    channelClose,                /* closeProc */
    nullptr,                     /* inputProc */
    ScriptWidget::channelOutput, /* outputProc */
    nullptr,                     /* seekProc */
    nullptr,                     /* setOptionProc */
    nullptr,                     /* getOptionProc */
    channelWatch,                /* watchProc */
    nullptr,                     /* getHandleProc */
    nullptr,                     /* close2Proc */
    nullptr,                     /* blockModeProc */
    nullptr,                     /* flushProc */
    nullptr,                     /* handlerProc */
    nullptr,                     /* wideSeekProc */
    nullptr,                     /* threadActionProc */
    nullptr                      /* truncateProc */
};

int ScriptWidget::tclExitHandler(ClientData instance_data,
                                 Tcl_Interp *interp,
                                 int argc,
                                 const char **argv) {
  ScriptWidget* widget = (ScriptWidget*) instance_data;
  // announces exit to Qt
  emit widget->tclExiting();
  // does not matter from here on, since GUI is getting ready exit
  return TCL_OK;
}

void ScriptWidget::setupTcl()
{
  interp_ = Tcl_CreateInterp();

  Tcl_Channel stdout_channel = Tcl_CreateChannel(
      &stdout_channel_type_, "stdout", (ClientData) this, TCL_WRITABLE);
  if (stdout_channel) {
    Tcl_SetChannelOption(nullptr, stdout_channel, "-translation", "lf");
    Tcl_SetChannelOption(nullptr, stdout_channel, "-buffering", "none");
    Tcl_RegisterChannel(interp_, stdout_channel);  // per man page: some tcl bug
    Tcl_SetStdChannel(stdout_channel, TCL_STDOUT);
  }

  // Overwrite exit to allow Qt to handle exit
  Tcl_CreateCommand(interp_, "exit", ScriptWidget::tclExitHandler, this, nullptr);

  // Ensures no newlines are present in stdout stream when using logger, but normal behavior in file writing
  Tcl_Eval(interp_, "rename puts ::tcl::openroad::puts");
  Tcl_Eval(interp_, "proc puts { args } { if {[llength $args] == 1} { ::tcl::openroad::puts -nonewline {*}$args } else { ::tcl::openroad::puts {*}$args } }");

  pauser_->setText("Running");
  pauser_->setStyleSheet("background-color: red");
  ord::tclAppInit(interp_);
  pauser_->setText("Idle");
  pauser_->setStyleSheet("");

  // TODO: tclAppInit should return the status which we could
  // pass to updateOutput
  addTclResultToOutput(TCL_OK);

  input_->init(interp_);
}

void ScriptWidget::executeCommand()
{
  pauser_->setText("Running");
  pauser_->setStyleSheet("background-color: red");
  QString command = input_->text();

  // Show the command that we executed
  addCommandToOutput(command);

  int return_code = Tcl_Eval(interp_, command.toLatin1().data());

  // Show its output
  addTclResultToOutput(return_code);

  if (return_code == TCL_OK) {
    // Update history; ignore repeated commands and keep last 100
    const int history_limit = 100;
    if (history_.empty() || command != history_.last()) {
      if (history_.size() == history_limit) {
        history_.pop_front();
      }

      history_.append(command);
    }
    historyPosition_ = history_.size();
  }

  pauser_->setText("Idle");
  pauser_->setStyleSheet("");

  emit commandExecuted(return_code);
}

void ScriptWidget::addCommandToOutput(const QString& cmd)
{
  const QString first_line_prefix    = ">>> ";
  const QString continue_line_prefix = "... ";

  QString command = first_line_prefix + cmd;
  command.replace("\n", "\n" + continue_line_prefix);

  addToOutput(command, cmd_msg_);
}

void ScriptWidget::addTclResultToOutput(int return_code)
{
  // Show the return value color-coded by ok/err.
  const char* result = Tcl_GetString(Tcl_GetObjResult(interp_));
  if (result[0] != '\0') {
    addToOutput(result, (return_code == TCL_OK) ? tcl_ok_msg_ : tcl_error_msg_);
  }
}

void ScriptWidget::addLogToOutput(const QString& text, const QColor& color)
{
  addToOutput(text, color);
}

void ScriptWidget::addReportToOutput(const QString& text)
{
  addToOutput(text, buffer_msg_);
}

void ScriptWidget::addToOutput(const QString& text, const QColor& color)
{
  // make sure cursor is at the end of the document
  output_->moveCursor(QTextCursor::End);

  QString output_text = text;
  if (text.endsWith('\n')) {
    // remove last new line
    output_text.chop(1);
  }

  // set new text color
  output_->setTextColor(color);

  QStringList output;
  for (QString& text_line : output_text.split('\n')) {
    // check for line length limits
    if (text_line.size() > max_output_line_length_) {
      text_line = text_line.left(max_output_line_length_-3);
      text_line += "...";
    }

    output.append(text_line);
  }
  // output new text
  output_->append(output.join("\n"));

  // ensure changes are updated
  QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
}

ScriptWidget::~ScriptWidget()
{
  // TODO: I am being lazy and not cleaning up the tcl interpreter.
  // We are likely exiting anyways
}

void ScriptWidget::goForwardHistory()
{
  if (historyPosition_ < history_.size() - 1) {
    ++historyPosition_;
    input_->setText(history_[historyPosition_]);
  } else if (historyPosition_ == history_.size() - 1) {
    ++historyPosition_;
    input_->setText(history_buffer_last_);
  }
}

void ScriptWidget::goBackHistory()
{
  if (historyPosition_ > 0) {
    if (historyPosition_ == history_.size()) {
      // whats in the buffer is the last thing the user was editing
      history_buffer_last_ = input_->text();
    }
    --historyPosition_;
    input_->setText(history_[historyPosition_]);
  }
}

void ScriptWidget::readSettings(QSettings* settings)
{
  settings->beginGroup("scripting");
  history_ = settings->value("history").toStringList();
  historyPosition_ = history_.size();

  input_->readSettings(settings);

  settings->endGroup();
}

void ScriptWidget::writeSettings(QSettings* settings)
{
  settings->beginGroup("scripting");
  settings->setValue("history", history_);

  input_->writeSettings(settings);

  settings->endGroup();
}

void ScriptWidget::pause()
{
  QString prior_text = pauser_->text();
  bool prior_enable = pauser_->isEnabled();
  QString prior_style = pauser_->styleSheet();
  pauser_->setText("Continue");
  pauser_->setStyleSheet("background-color: yellow");
  pauser_->setEnabled(true);
  paused_ = true;

  // Keep processing events until the user continues
  while (paused_) {
    QCoreApplication::processEvents(QEventLoop::WaitForMoreEvents);
  }

  pauser_->setText(prior_text);
  pauser_->setStyleSheet(prior_style);
  pauser_->setEnabled(prior_enable);

  // Make changes visible while command runs
  QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
}

void ScriptWidget::pauserClicked()
{
  paused_ = false;
}

void ScriptWidget::outputChanged()
{
  // ensure the new output is visible
  output_->ensureCursorVisible();
  // Make changes visible
  QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
}

void ScriptWidget::resizeEvent(QResizeEvent* event)
{
  input_->setMaximumHeight(event->size().height() - output_->sizeHint().height());
  QDockWidget::resizeEvent(event);
}

void ScriptWidget::setFont(const QFont& font)
{
  QDockWidget::setFont(font);
  output_->setFont(font);
  input_->setFont(font);
}

// This class is an spdlog sink that writes the messages into the output
// area.
template <typename Mutex>
class ScriptWidget::GuiSink : public spdlog::sinks::base_sink<Mutex>
{
 public:
  GuiSink(ScriptWidget* widget) : widget_(widget) {}

 protected:
  void sink_it_(const spdlog::details::log_msg& msg) override
  {
    // Convert the msg into a formatted string
    spdlog::memory_buf_t formatted;
    this->formatter_->format(msg, formatted);
    std::string formatted_msg = std::string(formatted.data(), formatted.size());

    if (msg.level == spdlog::level::level_enum::off) {
      // this comes from a ->report
      widget_->addReportToOutput(formatted_msg.c_str());
    }
    else {
      // select error message color if message level is error or above.
      const QColor& msg_color = msg.level >= spdlog::level::level_enum::err ? widget_->tcl_error_msg_ : widget_->buffer_msg_;

      widget_->addLogToOutput(formatted_msg.c_str(), msg_color);
    }
  }

  void flush_() override {}

 private:
  ScriptWidget* widget_;
};

void ScriptWidget::setLogger(utl::Logger* logger)
{
  logger_ = logger;
  logger->addSink(std::make_shared<GuiSink<std::mutex>>(this));
}

}  // namespace gui
<|endoftext|>


<commit_before>/*
 * REXI_CI.hpp
 *
 *  Created on: 18 Aug 2017
 *      Author: Martin Schreiber <SchreiberX@gmail.com>
 */

#ifndef SRC_INCLUDE_REXI_CI_HPP_
#define SRC_INCLUDE_REXI_CI_HPP_

#include <sweet/FatalError.hpp>
#include <libmath/DQStuff.hpp>
#include <rexi/REXIFunctions.hpp>
#include <vector>

template <
#if SWEET_QUADMATH
	typename T =  __float128
#else
	typename T =  double
#endif
>
class REXI_CI
{
	typedef std::complex<T> TComplex;

	std::vector<TComplex> alpha_eval;
	std::vector<TComplex> beta_eval;

	const T pi = DQStuff::fromString<T>("3.14159265358979323846264338327950288");
	const T pi2 = pi*DQStuff::fromString<T>("2.0");
	const T pi4 = pi*DQStuff::fromString<T>("4.0");
	const std::complex<T> I = std::complex<T>(0, 1);

public:
	std::vector<std::complex<double>> alpha;
	std::vector<std::complex<double>> beta;

	REXI_CI()
	{
	}

	void setup_shifted_circle(
			const std::string &i_function_name,
			int N,
			T max_real_evalue,
			T max_imag_evalue,
			T i_ci_gaussian_filter_scale_a,
			T i_ci_gaussian_filter_dt_norm,
			T i_ci_gaussian_filter_exp_N,
			T i_ci_gaussian_filter_dt
	)
	{
		/*
		 * See CI documentation
		 *
		 * Here we compute a circle which is given by the boundary points.
		 * This avoids large positive EValues leading to numerical cancellation effects
		 */
		T x0 = max_real_evalue;
		T xm = max_imag_evalue;
		T r = (x0*x0 + xm*xm)/(2.0*x0);
		T center = max_real_evalue-r;

		setup(
				i_function_name,
				N,
				"circle",
				r*2.0,
				r*2.0,
				center,
				i_ci_gaussian_filter_scale_a,
				i_ci_gaussian_filter_dt_norm,
				i_ci_gaussian_filter_exp_N,
				i_ci_gaussian_filter_dt
			);
	}

	void setup(
			const std::string &i_function_name,
			int N,
			const std::string &i_primitive_name,
			T i_size_real,
			T i_size_imag,
			T i_mu,
			T i_ci_gaussian_filter_scale_a,
			T i_ci_gaussian_filter_dt_norm,
			T i_ci_gaussian_filter_exp_N,
			T i_ci_gaussian_filter_dt
	)
	{
#if !SWEET_QUADMATH
//		FatalError("Don't use this without quad precision support to generate the coefficients!");
#endif

		alpha_eval.resize(N);
		beta_eval.resize(N);

		REXIFunctions<T> fun(i_function_name);

		if (i_primitive_name == "circle")
		{
			if (i_size_real != i_size_imag)
				FatalError("size along real and imaginary axis must be the same");

			T r = i_size_real*0.5;

			for (int j = 0; j < N; j++)
			{
//				T theta_j = (T)pi2*((T)j+(T)0.5)/(T)N;
				T theta_j = (T)pi2*((T)j+(T)0.5)/(T)N;

				// sampling position of support point
				TComplex pos = r*DQStuff::exp(I*theta_j);

				// shifted position
				TComplex gamma_j = pos + i_mu;

				/*
				 * Handle filter, see
				 * doc/rexi/filter
				 *
				 * We use a Gaussian bump to filter out the fast modes
				 */
				T filter_value = 1.0;
				if (i_ci_gaussian_filter_dt_norm != 0 && i_ci_gaussian_filter_scale_a != 0)
				{
//					T dist = DQStuff::sqrt(gamma_j.real()*gamma_j.real() + gamma_j.imag()*gamma_j.imag());
					T dist = DQStuff::abs(gamma_j.imag());

//					std::cout << (double)gamma_j.real() << std::endl;
//					std::cout << (double)gamma_j.imag() << std::endl;

					filter_value = DQStuff::exp(
										-DQStuff::pow(
												i_ci_gaussian_filter_dt_norm/(i_ci_gaussian_filter_scale_a*i_ci_gaussian_filter_dt)*dist,
												i_ci_gaussian_filter_exp_N
										)
									);
					std::cout << (double)dist << "\t" << (double)filter_value << std::endl;
				}

				beta_eval[j] = filter_value*fun.eval(gamma_j)*pos;
				alpha_eval[j] = gamma_j;

				beta_eval[j] /= (T)N;
			}
		}
		else if (i_primitive_name == "rectangle")
		{
//			FatalError("Not yet working :-(");

			T SRe = i_size_real;
			T SIm = i_size_imag;

			int NRe = 0.5 * SRe * N / (SRe + SIm);
			int NIm = 0.5 * SIm * N / (SRe + SIm);

#if 0
			/*
			 * Make N even numbered
			 */
			if (NRe & 1)
				NRe--;

			if (NIm & 1)
				NIm--;
#endif

			std::cout << "Size: " << (double)i_size_real << " x " << (double)i_size_imag << std::endl;
			std::cout << "Points: " << NRe << " x " << NIm << std::endl;
			int total_N = (2*NRe + 2*NIm);
			std::cout << "Points (total): " << total_N << std::endl;

			if (total_N > N)
				FatalError("Total number of points exceeds the total number of requested points");

			// Sampling distances
			T DRe = SRe/NRe;
			T DIm = SIm/NIm;

			// Sampling points along each rectangle boundary
			auto PRe = [&](int n) -> T { return (T)0.5*(DRe-SRe) + n*DRe; };
			auto PIm = [&](int n) -> T { return (T)0.5*(DIm-SIm) + n*DIm; };

			int j = 0;

			// BOTTOM
			for (int n = 0; n < NRe; n++)
			{
				TComplex z = -I*(T)0.5*SIm + PRe(n) + i_mu;

				beta_eval[j] = fun.eval(z);
				beta_eval[j] *= SRe/NRe;
				beta_eval[j] /= I*pi2;

				alpha_eval[j] = z;
				j++;
			}

			// RIGHT
			for (int n = 0; n < NIm; n++)
			{
				TComplex z = (T)0.5*SRe + I*PIm(n) + i_mu;

				beta_eval[j] = fun.eval(z);
				beta_eval[j] *= I;
				beta_eval[j] *= SIm/NIm;
				beta_eval[j] /= I*pi2;

				alpha_eval[j] = z;
				j++;
			}

			// TOP
			for (int n = 0; n < NRe; n++)
			{
				TComplex z = I*(T)0.5*SIm - PRe(n) + i_mu;

				beta_eval[j] = fun.eval(z);
				beta_eval[j] *= -(T)1.0;
				beta_eval[j] *= SRe/NRe;
				beta_eval[j] /= I*pi2;

				alpha_eval[j] = z;
				j++;
			}

			// LEFT
			for (int n = 0; n < NIm; n++)
			{
				TComplex z = -(T)0.5*SRe - I*PIm(n) + i_mu;

				beta_eval[j] = fun.eval(z);
				beta_eval[j] *= -I;
				beta_eval[j] *= SIm/NIm;
				beta_eval[j] /= I*pi2;

				alpha_eval[j] = z;
				j++;
			}

			N = j;
		}
		else
		{
			FatalError("This primitive is not known");
		}

		alpha.resize(N);
		beta.resize(N);

		for (int j = 0; j < N; j++)
		{
			alpha[j] = {(double)alpha_eval[j].real(), (double)alpha_eval[j].imag()};
			alpha[j] = -alpha[j];

			beta[j] = {(double)beta_eval[j].real(), (double)beta_eval[j].imag()};
			beta[j] = -beta[j];
//			std::cout << j << ":\t" << alpha[j] << std::endl;
		}
	}
};

#endif /* SRC_INCLUDE_REXI_CI_HPP_ */
<commit_msg>reenabled error if using REXI CI without quad precision<commit_after>/*
 * REXI_CI.hpp
 *
 *  Created on: 18 Aug 2017
 *      Author: Martin Schreiber <SchreiberX@gmail.com>
 */

#ifndef SRC_INCLUDE_REXI_CI_HPP_
#define SRC_INCLUDE_REXI_CI_HPP_

#include <sweet/FatalError.hpp>
#include <libmath/DQStuff.hpp>
#include <rexi/REXIFunctions.hpp>
#include <vector>

template <
#if SWEET_QUADMATH
	typename T =  __float128
#else
	typename T =  double
#endif
>
class REXI_CI
{
	typedef std::complex<T> TComplex;

	std::vector<TComplex> alpha_eval;
	std::vector<TComplex> beta_eval;

	const T pi = DQStuff::fromString<T>("3.14159265358979323846264338327950288");
	const T pi2 = pi*DQStuff::fromString<T>("2.0");
	const T pi4 = pi*DQStuff::fromString<T>("4.0");
	const std::complex<T> I = std::complex<T>(0, 1);

public:
	std::vector<std::complex<double>> alpha;
	std::vector<std::complex<double>> beta;

	REXI_CI()
	{
	}

	void setup_shifted_circle(
			const std::string &i_function_name,
			int N,
			T max_real_evalue,
			T max_imag_evalue,
			T i_ci_gaussian_filter_scale_a,
			T i_ci_gaussian_filter_dt_norm,
			T i_ci_gaussian_filter_exp_N,
			T i_ci_gaussian_filter_dt
	)
	{
		/*
		 * See CI documentation
		 *
		 * Here we compute a circle which is given by the boundary points.
		 * This avoids large positive EValues leading to numerical cancellation effects
		 */
		T x0 = max_real_evalue;
		T xm = max_imag_evalue;
		T r = (x0*x0 + xm*xm)/(2.0*x0);
		T center = max_real_evalue-r;

		setup(
				i_function_name,
				N,
				"circle",
				r*2.0,
				r*2.0,
				center,
				i_ci_gaussian_filter_scale_a,
				i_ci_gaussian_filter_dt_norm,
				i_ci_gaussian_filter_exp_N,
				i_ci_gaussian_filter_dt
			);
	}

	void setup(
			const std::string &i_function_name,
			int N,
			const std::string &i_primitive_name,
			T i_size_real,
			T i_size_imag,
			T i_mu,
			T i_ci_gaussian_filter_scale_a,
			T i_ci_gaussian_filter_dt_norm,
			T i_ci_gaussian_filter_exp_N,
			T i_ci_gaussian_filter_dt
	)
	{
#if !SWEET_QUADMATH
		FatalError("Don't use this without quad precision support to generate the coefficients!");
#endif

		alpha_eval.resize(N);
		beta_eval.resize(N);

		REXIFunctions<T> fun(i_function_name);

		if (i_primitive_name == "circle")
		{
			if (i_size_real != i_size_imag)
				FatalError("size along real and imaginary axis must be the same");

			T r = i_size_real*0.5;

			for (int j = 0; j < N; j++)
			{
//				T theta_j = (T)pi2*((T)j+(T)0.5)/(T)N;
				T theta_j = (T)pi2*((T)j+(T)0.5)/(T)N;

				// sampling position of support point
				TComplex pos = r*DQStuff::exp(I*theta_j);

				// shifted position
				TComplex gamma_j = pos + i_mu;

				/*
				 * Handle filter, see
				 * doc/rexi/filter
				 *
				 * We use a Gaussian bump to filter out the fast modes
				 */
				T filter_value = 1.0;
				if (i_ci_gaussian_filter_dt_norm != 0 && i_ci_gaussian_filter_scale_a != 0)
				{
//					T dist = DQStuff::sqrt(gamma_j.real()*gamma_j.real() + gamma_j.imag()*gamma_j.imag());
					T dist = DQStuff::abs(gamma_j.imag());

//					std::cout << (double)gamma_j.real() << std::endl;
//					std::cout << (double)gamma_j.imag() << std::endl;

					filter_value = DQStuff::exp(
										-DQStuff::pow(
												i_ci_gaussian_filter_dt_norm/(i_ci_gaussian_filter_scale_a*i_ci_gaussian_filter_dt)*dist,
												i_ci_gaussian_filter_exp_N
										)
									);
					std::cout << (double)dist << "\t" << (double)filter_value << std::endl;
				}

				beta_eval[j] = filter_value*fun.eval(gamma_j)*pos;
				alpha_eval[j] = gamma_j;

				beta_eval[j] /= (T)N;
			}
		}
		else if (i_primitive_name == "rectangle")
		{
//			FatalError("Not yet working :-(");

			T SRe = i_size_real;
			T SIm = i_size_imag;

			int NRe = 0.5 * SRe * N / (SRe + SIm);
			int NIm = 0.5 * SIm * N / (SRe + SIm);

#if 0
			/*
			 * Make N even numbered
			 */
			if (NRe & 1)
				NRe--;

			if (NIm & 1)
				NIm--;
#endif

			std::cout << "Size: " << (double)i_size_real << " x " << (double)i_size_imag << std::endl;
			std::cout << "Points: " << NRe << " x " << NIm << std::endl;
			int total_N = (2*NRe + 2*NIm);
			std::cout << "Points (total): " << total_N << std::endl;

			if (total_N > N)
				FatalError("Total number of points exceeds the total number of requested points");

			// Sampling distances
			T DRe = SRe/NRe;
			T DIm = SIm/NIm;

			// Sampling points along each rectangle boundary
			auto PRe = [&](int n) -> T { return (T)0.5*(DRe-SRe) + n*DRe; };
			auto PIm = [&](int n) -> T { return (T)0.5*(DIm-SIm) + n*DIm; };

			int j = 0;

			// BOTTOM
			for (int n = 0; n < NRe; n++)
			{
				TComplex z = -I*(T)0.5*SIm + PRe(n) + i_mu;

				beta_eval[j] = fun.eval(z);
				beta_eval[j] *= SRe/NRe;
				beta_eval[j] /= I*pi2;

				alpha_eval[j] = z;
				j++;
			}

			// RIGHT
			for (int n = 0; n < NIm; n++)
			{
				TComplex z = (T)0.5*SRe + I*PIm(n) + i_mu;

				beta_eval[j] = fun.eval(z);
				beta_eval[j] *= I;
				beta_eval[j] *= SIm/NIm;
				beta_eval[j] /= I*pi2;

				alpha_eval[j] = z;
				j++;
			}

			// TOP
			for (int n = 0; n < NRe; n++)
			{
				TComplex z = I*(T)0.5*SIm - PRe(n) + i_mu;

				beta_eval[j] = fun.eval(z);
				beta_eval[j] *= -(T)1.0;
				beta_eval[j] *= SRe/NRe;
				beta_eval[j] /= I*pi2;

				alpha_eval[j] = z;
				j++;
			}

			// LEFT
			for (int n = 0; n < NIm; n++)
			{
				TComplex z = -(T)0.5*SRe - I*PIm(n) + i_mu;

				beta_eval[j] = fun.eval(z);
				beta_eval[j] *= -I;
				beta_eval[j] *= SIm/NIm;
				beta_eval[j] /= I*pi2;

				alpha_eval[j] = z;
				j++;
			}

			N = j;
		}
		else
		{
			FatalError("This primitive is not known");
		}

		alpha.resize(N);
		beta.resize(N);

		for (int j = 0; j < N; j++)
		{
			alpha[j] = {(double)alpha_eval[j].real(), (double)alpha_eval[j].imag()};
			alpha[j] = -alpha[j];

			beta[j] = {(double)beta_eval[j].real(), (double)beta_eval[j].imag()};
			beta[j] = -beta[j];
//			std::cout << j << ":\t" << alpha[j] << std::endl;
		}
	}
};

#endif /* SRC_INCLUDE_REXI_CI_HPP_ */
<|endoftext|>


<commit_before>#include <node.h>
#include "include/cef_base.h"
#include "include/cef_browser.h"
#include "include/cef_frame.h"
#include "includes/cef_handler.h"
#include "includes/cef_sync_handler.h"
#include "base/native_window.h"
#include "appjs_window.h"

using namespace v8;
using namespace appjs;

int windowCount = 0;

ClientHandler::ClientHandler()
  : mainBrowserHandle(NULL) {
}

ClientHandler::~ClientHandler() {
}

Handle<Object> ClientHandler::GetNodeWindow(CefRefPtr<CefBrowser> browser) {
  return GetWindow(browser)->GetV8Handle();
}

Handle<Object> ClientHandler::CreatedBrowser(CefRefPtr<CefBrowser> browser) {
  NativeWindow* window = GetWindow(browser);
  window->SetBrowser(browser);
  return window->GetV8Handle();
}

NativeWindow* ClientHandler::GetWindow(CefRefPtr<CefBrowser> browser){
  return GetWindow(GetContainer(browser));
}

void ClientHandler::OnAfterCreated(CefRefPtr<CefBrowser> browser) {
  REQUIRE_UI_THREAD();
  AutoLock lock_scope(this);

  if (!browser->IsPopup()) {
    if (!mainBrowserHandle.get()) {
      mainBrowserHandle = browser;
    }
    windowCount++;
    CreatedBrowser(browser);
    GetWindow(browser)->Emit("create");
  }
}

bool ClientHandler::HasMainWindow() {
  return mainBrowserHandle != NULL && mainBrowserHandle.get() != NULL;
}

void ClientHandler::Shutdown() {
  if ( HasMainWindow() ) {
    mainBrowserHandle->CloseBrowser();
  }
}

void ClientHandler::OnContextCreated(CefRefPtr<CefBrowser> browser, CefRefPtr<CefFrame> frame, CefRefPtr<CefV8Context> context) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup()) {
    context->Enter();
    CefRefPtr<CefV8Value> appjsObj = CefV8Value::CreateObject(NULL);
    CefRefPtr<CefV8Value> func = CefV8Value::CreateFunction("send", new AppjsSyncHandler(browser));
    context->GetGlobal()->SetValue("appjs", appjsObj, V8_PROPERTY_ATTRIBUTE_NONE);
    appjsObj->SetValue("send", func, V8_PROPERTY_ATTRIBUTE_NONE);
    context->Exit();
    GetWindow(browser)->Emit("context-created");
  }
}

void ClientHandler::OnContextReleased(CefRefPtr<CefBrowser> browser, CefRefPtr<CefFrame> frame, CefRefPtr<CefV8Context> context) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup()) {
    GetWindow(browser)->Emit("context-released");
  }
}

bool ClientHandler::DoClose(CefRefPtr<CefBrowser> browser) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup() && --windowCount == 0) {
    Local<Object> global = Context::GetCurrent()->Global();
    Local<Object> process = global->Get(String::NewSymbol("process"))->ToObject();
    Local<Object> emitter = Local<Object>::Cast(process->Get(String::NewSymbol("AppjsEmitter")));
    Handle<Value> exitArgv[1] = {String::New("exit")};
    node::MakeCallback(emitter,"emit",1,exitArgv);
    Cef::Shutdown();
    mainBrowserHandle = NULL;
    return true;
  } else {
    return false;
  }
}

void ClientHandler::OnBeforeClose(CefRefPtr<CefBrowser> browser) {
  REQUIRE_UI_THREAD();

  if(!browser->IsPopup()) {
    NativeWindow* window = GetWindow(browser);
    window->PrepareClose();
    DoClose(browser);
#ifdef __WIN__
    delete GetWindow(browser);
#endif
  }
}

void ClientHandler::OnLoadEnd(CefRefPtr<CefBrowser> browser, CefRefPtr<CefFrame> frame, int httpStatusCode) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup()) {
    GetWindow(browser)->Emit("ready");
  }
}

void ClientHandler::OnContentsSizeChange(CefRefPtr<CefBrowser> browser, CefRefPtr<CefFrame> frame, int width, int height) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup()) {
    NativeWindow* window = GetWindow(browser);
    if (window->GetAutoResize()) {
      window->Resize(width, height);
    }
  }
}

void ClientHandler::OnTitleChange(CefRefPtr<CefBrowser> browser, const CefString& title) {
  REQUIRE_UI_THREAD();
  std::string titleStr(title);
  NativeWindow::SetWindowTitle(GetContainer(browser),titleStr.c_str());
  if (!browser->IsPopup()) {
    GetWindow(browser)->Emit("title-change");
  }
}
<commit_msg>Check nativewindow existance before resizing the window<commit_after>#include <node.h>
#include "include/cef_base.h"
#include "include/cef_browser.h"
#include "include/cef_frame.h"
#include "includes/cef_handler.h"
#include "includes/cef_sync_handler.h"
#include "base/native_window.h"
#include "appjs_window.h"

using namespace v8;
using namespace appjs;

int windowCount = 0;

ClientHandler::ClientHandler()
  : mainBrowserHandle(NULL) {
}

ClientHandler::~ClientHandler() {
}

Handle<Object> ClientHandler::GetNodeWindow(CefRefPtr<CefBrowser> browser) {
  return GetWindow(browser)->GetV8Handle();
}

Handle<Object> ClientHandler::CreatedBrowser(CefRefPtr<CefBrowser> browser) {
  NativeWindow* window = GetWindow(browser);
  window->SetBrowser(browser);
  return window->GetV8Handle();
}

NativeWindow* ClientHandler::GetWindow(CefRefPtr<CefBrowser> browser){
  return GetWindow(GetContainer(browser));
}

void ClientHandler::OnAfterCreated(CefRefPtr<CefBrowser> browser) {
  REQUIRE_UI_THREAD();
  AutoLock lock_scope(this);

  if (!browser->IsPopup()) {
    if (!mainBrowserHandle.get()) {
      mainBrowserHandle = browser;
    }
    windowCount++;
    CreatedBrowser(browser);
    GetWindow(browser)->Emit("create");
  }
}

bool ClientHandler::HasMainWindow() {
  return mainBrowserHandle != NULL && mainBrowserHandle.get() != NULL;
}

void ClientHandler::Shutdown() {
  if ( HasMainWindow() ) {
    mainBrowserHandle->CloseBrowser();
  }
}

void ClientHandler::OnContextCreated(CefRefPtr<CefBrowser> browser, CefRefPtr<CefFrame> frame, CefRefPtr<CefV8Context> context) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup()) {
    context->Enter();
    CefRefPtr<CefV8Value> appjsObj = CefV8Value::CreateObject(NULL);
    CefRefPtr<CefV8Value> func = CefV8Value::CreateFunction("send", new AppjsSyncHandler(browser));
    context->GetGlobal()->SetValue("appjs", appjsObj, V8_PROPERTY_ATTRIBUTE_NONE);
    appjsObj->SetValue("send", func, V8_PROPERTY_ATTRIBUTE_NONE);
    context->Exit();
    GetWindow(browser)->Emit("context-created");
  }
}

void ClientHandler::OnContextReleased(CefRefPtr<CefBrowser> browser, CefRefPtr<CefFrame> frame, CefRefPtr<CefV8Context> context) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup()) {
    GetWindow(browser)->Emit("context-released");
  }
}

bool ClientHandler::DoClose(CefRefPtr<CefBrowser> browser) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup() && --windowCount == 0) {
    Local<Object> global = Context::GetCurrent()->Global();
    Local<Object> process = global->Get(String::NewSymbol("process"))->ToObject();
    Local<Object> emitter = Local<Object>::Cast(process->Get(String::NewSymbol("AppjsEmitter")));
    Handle<Value> exitArgv[1] = {String::New("exit")};
    node::MakeCallback(emitter,"emit",1,exitArgv);
    Cef::Shutdown();
    mainBrowserHandle = NULL;
    return true;
  } else {
    return false;
  }
}

void ClientHandler::OnBeforeClose(CefRefPtr<CefBrowser> browser) {
  REQUIRE_UI_THREAD();

  if(!browser->IsPopup()) {
    NativeWindow* window = GetWindow(browser);
    window->PrepareClose();
    DoClose(browser);
#ifdef __WIN__
    delete GetWindow(browser);
#endif
  }
}

void ClientHandler::OnLoadEnd(CefRefPtr<CefBrowser> browser, CefRefPtr<CefFrame> frame, int httpStatusCode) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup()) {
    GetWindow(browser)->Emit("ready");
  }
}

void ClientHandler::OnContentsSizeChange(CefRefPtr<CefBrowser> browser, CefRefPtr<CefFrame> frame, int width, int height) {
  REQUIRE_UI_THREAD();
  if (!browser->IsPopup()) {
    NativeWindow* window = GetWindow(browser);
    if (window != NULL && window->GetAutoResize()) {
      window->Resize(width, height);
    }
  }
}

void ClientHandler::OnTitleChange(CefRefPtr<CefBrowser> browser, const CefString& title) {
  REQUIRE_UI_THREAD();
  std::string titleStr(title);
  NativeWindow::SetWindowTitle(GetContainer(browser),titleStr.c_str());
  if (!browser->IsPopup()) {
    GetWindow(browser)->Emit("title-change");
  }
}
<|endoftext|>