<commit_before>#include <ensenso_nx/ensenso_nx_node.h>

namespace ensenso_nx
{

EnsensoNxNode::EnsensoNxNode():
	nh__()
{
	is_params_loaded__ = false;

	if(!nh__.getParam("serial_number", serial_number__))
		ROS_ERROR_NAMED("EnensoNxNode", "Serial number for Ensenso camera has not been set, the camera won't be able to open");

	run_mode__ = SERVER_MODE;
	if( !nh__.getParam("run_mode", run_mode__) )
		ROS_WARN_NAMED("EnensoNxNode", "Run mode param is not set, the camera will start in SERVER mode by default");

	rate__ = 1.0;
	if( !nh__.getParam("rate", rate__) )
		ROS_WARN_NAMED("EnensoNxNode", "Rate param is not set, it will be set to 1Hz");

	frame_name__ = "single_ensenso_n35_body";
	if( !nh__.getParam("frame_name", frame_name__) )
		ROS_WARN_NAMED("EnensoNxNode", "frame_name is not set, it will be set to single_ensenso_n35_body");

	capture_params__.auto_exposure = false;
	if( !nh__.getParam("auto_exposure", capture_params__.auto_exposure) )
		ROS_WARN_NAMED("EnensoNxNode", "auto_exposure is not set, it will be set to false");

	int exposure_time = 0;
	if( !nh__.getParam("exposure_time", exposure_time) )
		ROS_WARN_NAMED("EnensoNxNode", "exposure_time is not set, it will be set to 0 (zero)");
	capture_params__.exposure_time = static_cast<unsigned int>(exposure_time);

	capture_params__.dense_cloud = false;
	if( !nh__.getParam("dense_cloud", capture_params__.dense_cloud) )
		ROS_WARN_NAMED("EnensoNxNode", "dense_cloud is not set, it will be set to 0 (false)");

	is_params_loaded__ = true;

	set_camera_service__ = nh__.advertiseService("set_camera", &EnsensoNxNode::setCameraEnable, this);

	cloud_publisher__ = nh__.advertise<pcl::PointCloud<pcl::PointXYZ> >("ensenso_cloud", 1);

	cloud_server__ = nh__.advertiseService("ensenso_server", &EnsensoNxNode::pointCloudServiceCallback, this);

	publisher_clock__ = nh__.createTimer( ros::Rate(rate__), &EnsensoNxNode::publisherCallBack, this, false, false);

	configure_server__.setCallback( boost::bind(&EnsensoNxNode::reconfigureCallback, this, _1, _2) );

	std::cout << "ROS EnsensoNxNode Settings: " << std::endl;
	std::cout << "\trun mode: \t" << run_mode__<< std::endl;
	std::cout << "\tframe name: \t" << frame_name__ << std::endl;
	if ( run_mode__ == PUBLISHER_MODE )
	{
		std::cout << "\trate [hz]: \t" << rate__  << std::endl;
		std::cout << "\tauto_exposure [t/f]: \t" << capture_params__.auto_exposure << std::endl;
		if ( !capture_params__.auto_exposure )
		{
			std::cout << "\texposure [ms]: \t" << capture_params__.exposure_time << std::endl;
		}
		std::cout << "\tdense_cloud: [t/f] \t" << capture_params__.dense_cloud << std::endl;
	}
}

EnsensoNxNode::~EnsensoNxNode()
{
	std_srvs::SetBool close_camera;
	close_camera.request.data = false;
	setCameraEnable(close_camera.request, close_camera.response);
}

void EnsensoNxNode::reconfigureCallback(EnsensoNxParamsConfig& __config, uint32_t __level)
{
	switch(__level)
	{
	case 0:
	{
		frame_name__ = __config.frame_name;
		std_srvs::SetBool enable_camera;
		enable_camera.request.data = __config.enable_camera;
		setCameraEnable(enable_camera.request, enable_camera.response);
	}
	break;
	case 1:
	{
		if( is_camera_enabled__ )
		{
			if( run_mode__ == PUBLISHER_MODE )
			{
				publisher_clock__.stop();
				publisher_clock__.setPeriod( ros::Duration( static_cast<double>(__config.exposure_time) ), false );
			}
			capture_params__.auto_exposure = __config.auto_exposure;
			capture_params__.exposure_time = static_cast<unsigned int>(__config.exposure_time);
			capture_params__.dense_cloud = __config.dense_cloud;

			run_mode__ = __config.run_mode;

			grab_locker__.lock();
			camera__->configureCapture(capture_params__);
			grab_locker__.unlock();

			if(run_mode__ == PUBLISHER_MODE )
				publisher_clock__.start();
		}
		else
		{
			ROS_WARN("EnsensoNxNode ensenso_server service: Camera hasn't been initiliazed, call set_camera to enable it");
		}
	}
	break;
	default:
	{
		ROS_WARN("Dynamic reconfigure callback ran but no reconfiguration was done");
	}
	break;
	}

}

bool EnsensoNxNode::setCameraEnable(std_srvs::SetBoolRequest& __req, std_srvs::SetBoolResponse& __res)
{
	// if called again, and the camera is already opened, it will destroy/close and reopen again
	grab_locker__.lock();
	if( __req.data )
	{
		camera__.reset(new Device(serial_number__));
		if( camera__ )
		{
			camera__->configureCapture(capture_params__);
			is_camera_enabled__ = true;
		}
	}
	if( !__req.data )
	{
		camera__.reset();
		is_camera_enabled__ = false;
	}
	grab_locker__.unlock();
	return true;
}

bool EnsensoNxNode::grabCloud()<commit_msg>* Adds autostart to be copatible with current dependant code * Adds information prints regarding the dynamic reconfigure changes * Fix error in setting the period of the publisher * Only open the camera if it is not already opened * Send info back to response of open/close service<commit_after>#include <ensenso_nx/ensenso_nx_node.h>

namespace ensenso_nx
{

EnsensoNxNode::EnsensoNxNode():
	nh__()
{
	is_params_loaded__ = false;

	if(!nh__.getParam("serial_number", serial_number__))
		ROS_ERROR_NAMED("EnensoNxNode", "Serial number for Ensenso camera has not been set, the camera won't be able to open");

	run_mode__ = SERVER_MODE;
	if( !nh__.getParam("run_mode", run_mode__) )
		ROS_WARN_NAMED("EnensoNxNode", "Run mode param is not set, the camera will start in SERVER mode by default");

	rate__ = 1.0;
	if( !nh__.getParam("rate", rate__) )
		ROS_WARN_NAMED("EnensoNxNode", "Rate param is not set, it will be set to 1Hz");

	frame_name__ = "single_ensenso_n35_body";
	if( !nh__.getParam("frame_name", frame_name__) )
		ROS_WARN_NAMED("EnensoNxNode", "frame_name is not set, it will be set to single_ensenso_n35_body");

	capture_params__.auto_exposure = false;
	if( !nh__.getParam("auto_exposure", capture_params__.auto_exposure) )
		ROS_WARN_NAMED("EnensoNxNode", "auto_exposure is not set, it will be set to false");

	int exposure_time = 0;
	if( !nh__.getParam("exposure_time", exposure_time) )
		ROS_WARN_NAMED("EnensoNxNode", "exposure_time is not set, it will be set to 0 (zero)");
	capture_params__.exposure_time = static_cast<unsigned int>(exposure_time);

	capture_params__.dense_cloud = false;
	if( !nh__.getParam("dense_cloud", capture_params__.dense_cloud) )
		ROS_WARN_NAMED("EnensoNxNode", "dense_cloud is not set, it will be set to 0 (false)");

	is_params_loaded__ = true;

	set_camera_service__ = nh__.advertiseService("set_camera", &EnsensoNxNode::setCameraEnable, this);

	cloud_publisher__ = nh__.advertise<pcl::PointCloud<pcl::PointXYZ> >("ensenso_cloud", 1);

	cloud_server__ = nh__.advertiseService("ensenso_server", &EnsensoNxNode::pointCloudServiceCallback, this);

	publisher_clock__ = nh__.createTimer( ros::Rate(rate__), &EnsensoNxNode::publisherCallBack, this, false, false);

	configure_server__.setCallback( boost::bind(&EnsensoNxNode::reconfigureCallback, this, _1, _2) );

	std::cout << "ROS EnsensoNxNode Settings: " << std::endl;
	std::cout << "\trun mode: \t" << run_mode__<< std::endl;
	std::cout << "\tframe name: \t" << frame_name__ << std::endl;
	if ( run_mode__ == PUBLISHER_MODE )
	{
		std::cout << "\trate [hz]: \t" << rate__  << std::endl;
		std::cout << "\tauto_exposure [t/f]: \t" << capture_params__.auto_exposure << std::endl;
		if ( !capture_params__.auto_exposure )
		{
			std::cout << "\texposure [ms]: \t" << capture_params__.exposure_time << std::endl;
		}
		std::cout << "\tdense_cloud: [t/f] \t" << capture_params__.dense_cloud << std::endl;
	}

	// autostart
	is_camera_enabled__ = false;
	std_srvs::SetBool enable_camera;
	enable_camera.request.data = true;
	setCameraEnable(enable_camera.request, enable_camera.response);
}

EnsensoNxNode::~EnsensoNxNode()
{
	if(is_camera_enabled__)
	{
		std_srvs::SetBool close_camera;
		close_camera.request.data = false;
		setCameraEnable(close_camera.request, close_camera.response);
	}
}

void EnsensoNxNode::reconfigureCallback(EnsensoNxParamsConfig& __config, uint32_t __level)
{
	switch(__level)
	{
	case 0:
	{
		std_srvs::SetBool enable_camera;
		if( __config.enable_camera )
			enable_camera.request.data = true;
		else
			enable_camera.request.data = false;
		setCameraEnable(enable_camera.request, enable_camera.response);
		break;
	}
	case 1:
	{
		if( is_camera_enabled__ )
		{
			if( run_mode__ == PUBLISHER_MODE )
			{
				publisher_clock__.stop();
				publisher_clock__.setPeriod( ros::Duration( static_cast<double>(__config.rate) ), false );
			}
			capture_params__.auto_exposure = __config.auto_exposure;
			capture_params__.exposure_time = static_cast<unsigned int>(__config.exposure_time);
			capture_params__.dense_cloud = __config.dense_cloud;

			run_mode__ = __config.run_mode;

			grab_locker__.lock();
			camera__->configureCapture(capture_params__);
			grab_locker__.unlock();

			if(run_mode__ == PUBLISHER_MODE )
			{
				ROS_INFO("EnsensoNxNode: Mode changed to PUBLISHER_MODE");
				publisher_clock__.start();
			}
			else
			{
				ROS_INFO("EnsensoNxNode: Mode changed to SERVER_MODE");
			}
		}
		else
		{
			ROS_WARN("EnsensoNxNode ensenso_server service: Camera hasn't been initiliazed, call set_camera to enable it");
		}
		break;
	}
	case 2:
	{
		frame_name__ = __config.frame_name;
		ROS_INFO_STREAM("EnsensoNxNode: Frame ID in cloud message changed to [\x1B[37m" << frame_name__ << "\x1B[0m" << "]");
		break;
	}
	default:
	{
		ROS_WARN("Dynamic reconfigure callback ran but no reconfiguration was done");
		break;
	}
	}

}

bool EnsensoNxNode::setCameraEnable(std_srvs::SetBoolRequest& __req, std_srvs::SetBoolResponse& __res)
{
	// if called again, and the camera is already opened, it will destroy/close and reopen again
	grab_locker__.lock();
	if( __req.data && !is_camera_enabled__ )
	{
		camera__.reset(new Device(serial_number__));
		if( camera__ )
		{
			camera__->configureCapture(capture_params__);
			is_camera_enabled__ = true;
			__res.message = "EnsensoNx is Ready";
		}
	}
	else
	{
		camera__.reset();
		is_camera_enabled__ = false;
		__res.message = "EnsensoNx is closed";
	}
	grab_locker__.unlock();
	__res.success = true;
	return true;
}

bool EnsensoNxNode::grabCloud()
{'fraction_non_alphanumeric': 0.12555654496883348, 'fraction_numerical': 0.003957653111704759, 'mean_word_length': 5.434118395926162, 'pattern_counts': {'":': 0, '<': 101, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4992418', 'n_tokens_mistral': 3957, 'n_tokens_neox': 3583, 'n_words': 739}