<commit_before>#include <ros/ros.h>
#include "outdoor_bot/radar_msg.h"
#include "rcmRadar/radar.h"
#include "outdoor_bot_defines.h"

#define RADAR_WAIT_TIME 0.5
#define HOME_RADAR_SEPARATION 1000 //1800 // distance between radars on home platform, in mm
#define BOT_RADAR_SEPARATION 1000 //1400

using namespace std;

class radar
{

private:

	radarRanger leftRanger_, rightRanger_;
	double distanceFromLeftToLeft_, distanceFromLeftToRight_, distanceFromRightToLeft_, distanceFromRightToRight_;
	double distanceToHome_, angleToHome_;

public:
	radar()
		: leftRanger_("/dev/ttyRadar_103"),
		  rightRanger_("/dev/ttyRadar_100"),
		  distanceFromLeftToLeft_(0), distanceFromLeftToRight_(0.), distanceFromRightToLeft_(0.), distanceFromRightToRight_(0),
		  distanceToHome_(0.),
		  angleToHome_(0)
	{
	}

void getLocation()
{
	if ( distanceFromLeftToLeft_ < 0.1 ||
		distanceFromLeftToRight_ < 0.1 ||
		distanceFromRightToLeft_ < 0.1 ||
		distanceFromRightToRight_ < 0.1) 
		{
			distanceToHome_ = 0.;
			angleToHome_ = 0.;
			cout << "unable to get some radar ranges, cannot calculate location" << endl;
			return;
		}
		
	double cosLeftHomeToLeftBot = 
		( (distanceFromLeftToLeft_ * distanceFromLeftToLeft_) 
		+ (HOME_RADAR_SEPARATION * HOME_RADAR_SEPARATION)
		- (distanceFromLeftToRight_ * distanceFromLeftToRight_) )
		/  ( 2. * distanceFromLeftToLeft_ * HOME_RADAR_SEPARATION);
	
	double cosLeftBotToRightHome =
	   ( (distanceFromLeftToRight_ * distanceFromLeftToRight_)
		+ (BOT_RADAR_SEPARATION * BOT_RADAR_SEPARATION)
		- (distanceFromRightToRight_ * distanceFromRightToRight_) )
		/  ( 2. * distanceFromLeftToRight_ * BOT_RADAR_SEPARATION);
		
	double botOrientation = 1.57 + acos(cosLeftHomeToLeftBot) + acos(cosLeftBotToRightHome);
	
	double distanceFromLeftToCenterSquared = 
		( (distanceFromLeftToLeft_ * distanceFromLeftToLeft_) 
		+ ((HOME_RADAR_SEPARATION * HOME_RADAR_SEPARATION)/4.) )
		- (distanceFromLeftToLeft_ * HOME_RADAR_SEPARATION * cosLeftHomeToLeftBot);
		
	double distanceFromLeftToCenter;
	
	if (distanceFromLeftToCenterSquared >= 0) distanceFromLeftToCenter = sqrt(distanceFromLeftToCenterSquared);
	else 
	{
		cout << "distanceFromLeftToCenterSquared was < 0 " << endl;
		return;
	}
	
	double cosAngleE = 
		( (distanceFromLeftToLeft_ * distanceFromLeftToLeft_) 
		+ (distanceFromLeftToCenter * distanceFromLeftToCenter)
		- ((HOME_RADAR_SEPARATION * HOME_RADAR_SEPARATION)/4.) )
		/ ( 2. * distanceFromLeftToLeft_ * distanceFromLeftToCenter);
	
	double anglef = acos(cosLeftHomeToLeftBot) - (botOrientation + 1.57);
	double angleh = 3.14 - (acos(cosAngleE) + anglef);
	
	double distanceFromCenterToCenterSquared = 
		((BOT_RADAR_SEPARATION * BOT_RADAR_SEPARATION)/4.)
		+ (distanceFromLeftToCenter * distanceFromLeftToCenter)
		- (BOT_RADAR_SEPARATION * distanceFromLeftToCenter * cos(angleh));
		
	double anglei = 
		( (distanceFromLeftToCenter * distanceFromLeftToCenter)
		+ ((BOT_RADAR_SEPARATION * BOT_RADAR_SEPARATION)/4.) 
		- distanceFromCenterToCenterSquared ) 
		/ (distanceFromLeftToCenter * BOT_RADAR_SEPARATION);
		
	double angleToHome_ = (1.57 - anglei) * 57.3;	// convert to degrees
	
	// convert to meters
	if  (distanceFromCenterToCenterSquared >= 0) distanceToHome_ = sqrt(distanceFromCenterToCenterSquared) / 1000. ;
	else cout << "distanceFromCenterToCenterSquared was negative, = " << distanceFromCenterToCenterSquared << endl;
	
	cout << "cosLeftHomeToLeftBot = " << cosLeftHomeToLeftBot << ", an angle of " << acos(cosLeftHomeToLeftBot) * 57.3 << " degrees" << endl;
	cout << "cosLeftBotToRightHome = " << cosLeftBotToRightHome << ", an angle of " << acos(cosLeftBotToRightHome) * 57.3 << " degrees" << endl;
	cout << "botOrientation = " << botOrientation * 57.3 << " degrees " << endl;
	cout << "distanceFromLeftToCenterSquared = " << distanceFromLeftToCenterSquared << endl;
	cout << "distanceFromLeftToCenter = " << distanceFromLeftToCenter << endl;
	cout << "cosAngleE = " << cosAngleE << ", an angle of " << acos(cosAngleE) * 57.3 << " degrees" << endl;
	cout << "anglef = " << anglef * 57.3 << endl;
	cout << "angleh = " << angleh * 57.3 << endl;
	cout << "distanceFromCenterToCenterSquared = " << distanceFromCenterToCenterSquared << endl;	
	if (distanceFromCenterToCenterSquared >= 0)
		cout << "distanceFromCenterToCenter = " << sqrt(distanceFromCenterToCenterSquared) << endl;
	else cout << "distanceFromCenterToCenterSquared is a negative number " << endl;
	cout << "anglei = " << anglei * 57.3 << endl;
	cout << "angleToHome_ = " << angleToHome_ << endl;
	
	cout << "Home is " << distanceToHome_ << " meters away at an angle = "
			<< angleToHome_ << endl;	
}

double getDistanceFromLeftToLeft() { return distanceFromLeftToLeft_; }
double getDistanceFromLeftToRight() { return distanceFromLeftToRight_; }
double getDistanceFromRightToLeft() { return distanceFromRightToLeft_; }
double getDistanceFromRightToRight() { return distanceFromRightToRight_; }
double getDistanceToHome() { return distanceToHome_; }
double getAngleToHome() { return angleToHome_; }


void getRadarRanges()
{
	ros::Time last_time;
   ros::Time current_time = ros::Time::now();
   
   distanceFromLeftToLeft_ = leftRanger_.getRange(LEFT_RADAR_NUMBER);
	last_time = ros::Time::now();
	while ( current_time.toSec() - last_time.toSec() < RADAR_WAIT_TIME) current_time = ros::Time::now(); // delay needed for radars to get data

	distanceFromLeftToRight_ = leftRanger_.getRange(RIGHT_RADAR_NUMBER);
	last_time = ros::Time::now();
	while ( current_time.toSec() - last_time.toSec() < RADAR_WAIT_TIME) current_time = ros::Time::now(); 
	
	distanceFromRightToLeft_ = rightRanger_.getRange(LEFT_RADAR_NUMBER);
	last_time = ros::Time::now();
	while ( current_time.toSec() - last_time.toSec() < RADAR_WAIT_TIME) current_time = ros::Time::now(); 
	
	distanceFromRightToRight_ = rightRanger_.getRange(RIGHT_RADAR_NUMBER);
	last_time = ros::Time::now();
	while ( current_time.toSec() - last_time.toSec() < RADAR_WAIT_TIME) current_time = ros::Time::now();
}

void testDataRadarRanges()
{  
   distanceFromLeftToLeft_ = 2000.;
	distanceFromLeftToRight_ = 2236.;
	distanceFromRightToLeft_ = 2236.;
	distanceFromRightToRight_ = 2000.;
}
	
};

int main(int argc, char** argv)
{<commit_msg>radar_node working nicely with test data<commit_after>#include <ros/ros.h>
#include "outdoor_bot/radar_msg.h"
#include "rcmRadar/radar.h"
#include "outdoor_bot_defines.h"

#define RADAR_WAIT_TIME 0.5
#define HOME_RADAR_SEPARATION 1900 // distance between radars on home platform, in mm
#define BOT_RADAR_SEPARATION 1092

using namespace std;

class radar
{

private:

	radarRanger leftRanger_, rightRanger_;
	double distanceFromLeftToLeft_, distanceFromLeftToRight_, distanceFromRightToLeft_, distanceFromRightToRight_;
	double distanceToHome_, angleToHome_;

public:
	radar()
		: leftRanger_("/dev/ttyRadar_103"),
		  rightRanger_("/dev/ttyRadar_100"),
		  distanceFromLeftToLeft_(0), distanceFromLeftToRight_(0.), distanceFromRightToLeft_(0.), distanceFromRightToRight_(0),
		  distanceToHome_(0.),
		  angleToHome_(0)
	{
	}

void getLocation()
{
	if ( distanceFromLeftToLeft_ < 0.1 ||
		distanceFromLeftToRight_ < 0.1 ||
		distanceFromRightToLeft_ < 0.1 ||
		distanceFromRightToRight_ < 0.1) 
		{
			distanceToHome_ = 0.;
			angleToHome_ = 0.;
			cout << "unable to get some radar ranges, cannot calculate location" << endl;
			return;
		}
		
	double cosLeftHomeToLeftBot =
	   ( (distanceFromLeftToLeft_ * distanceFromLeftToLeft_)
		+ (HOME_RADAR_SEPARATION * HOME_RADAR_SEPARATION)
		- (distanceFromLeftToRight_ * distanceFromLeftToRight_) )
		/  ( 2. * distanceFromLeftToLeft_ * HOME_RADAR_SEPARATION);
		
	double cosAngleB = 
		( (distanceFromLeftToRight_ * distanceFromLeftToRight_) 
		+ (BOT_RADAR_SEPARATION * BOT_RADAR_SEPARATION)
		- (distanceFromRightToRight_ * distanceFromRightToRight_) )
		/  ( 2. * distanceFromLeftToRight_ * BOT_RADAR_SEPARATION);
		
	double cosAngleD =
		( (distanceFromLeftToLeft_ * distanceFromLeftToLeft_) 
		+ (distanceFromLeftToRight_ * distanceFromLeftToRight_)
		- (HOME_RADAR_SEPARATION * HOME_RADAR_SEPARATION) )
		/  ( 2. * distanceFromLeftToLeft_ * distanceFromLeftToRight_);	
		
	double botOrientation = -3.14 + acos(cosAngleB) + acos(cosAngleD) + acos(cosLeftHomeToLeftBot); 
	
	double distanceFromLeftToCenterSquared = 
		( (distanceFromLeftToLeft_ * distanceFromLeftToLeft_) 
		+ ((HOME_RADAR_SEPARATION * HOME_RADAR_SEPARATION)/4.) )
		- (distanceFromLeftToLeft_ * HOME_RADAR_SEPARATION * cosLeftHomeToLeftBot);
		
	double distanceFromLeftToCenter;
	
	if (distanceFromLeftToCenterSquared >= 0) distanceFromLeftToCenter = sqrt(distanceFromLeftToCenterSquared);
	else 
	{
		cout << "distanceFromLeftToCenterSquared was < 0 " << endl;
		return;
	}
	
	double cosAngleE = 
		( (distanceFromLeftToLeft_ * distanceFromLeftToLeft_) 
		+ (distanceFromLeftToCenter * distanceFromLeftToCenter)
		- ((HOME_RADAR_SEPARATION * HOME_RADAR_SEPARATION)/4.) )
		/ ( 2. * distanceFromLeftToLeft_ * distanceFromLeftToCenter);
	
	double anglef = acos(cosLeftHomeToLeftBot) - botOrientation;
	double angleh = 3.14 - (acos(cosAngleE) + anglef);
	
	double distanceFromCenterToCenterSquared = 
		((BOT_RADAR_SEPARATION * BOT_RADAR_SEPARATION)/4.)
		+ (distanceFromLeftToCenter * distanceFromLeftToCenter)
		- (BOT_RADAR_SEPARATION * distanceFromLeftToCenter * cos(angleh));

	double distanceFromCenterToCenter;
	if (distanceFromCenterToCenterSquared >= 0) distanceFromCenterToCenter = sqrt(distanceFromCenterToCenterSquared);
	else 
	{
		cout << "distanceFromRightToCenterSquared was < 0 " << endl;
		return;
	}		
	
	double cosAngleI = 
		( (distanceFromCenterToCenter * distanceFromCenterToCenter)
		+ ((BOT_RADAR_SEPARATION * BOT_RADAR_SEPARATION)/4.) 
		- distanceFromLeftToCenterSquared ) 
		/ (distanceFromCenterToCenter * BOT_RADAR_SEPARATION);
		
	double angleToHome_ = (1.57 - acos(cosAngleI)) * 57.3;	// convert to degrees
	
	// convert to meters
	if  (distanceFromCenterToCenterSquared >= 0) distanceToHome_ = sqrt(distanceFromCenterToCenterSquared) / 1000. ;
	else cout << "distanceFromCenterToCenterSquared was negative, = " << distanceFromCenterToCenterSquared << endl;
	
	cout << "cosLeftHomeToLeftBot = " << cosLeftHomeToLeftBot << ", an angle of " << acos(cosLeftHomeToLeftBot) * 57.3 << " degrees" << endl;
	cout << "cosAngleB = " << cosAngleB << ", an angle of " << acos(cosAngleB) * 57.3 << " degrees" << endl;
	cout << "cosAngleD = " << cosAngleD << ", an angle of " << acos(cosAngleD) * 57.3 << " degrees" << endl;
	cout << "botOrientation = " << botOrientation * 57.3 << " degrees " << endl;
	cout << "distanceFromLeftToCenterSquared = " << distanceFromLeftToCenterSquared << endl;
	cout << "distanceFromLeftToCenter = " << distanceFromLeftToCenter << endl;
	cout << "cosAngleE = " << cosAngleE << ", an angle of " << acos(cosAngleE) * 57.3 << " degrees" << endl;
	cout << "anglef = " << anglef * 57.3 << endl;
	cout << "angleh = " << angleh * 57.3 << endl;
	cout << "distanceFromCenterToCenterSquared = " << distanceFromCenterToCenterSquared << endl;	
	if (distanceFromCenterToCenterSquared >= 0)
		cout << "distanceFromCenterToCenter = " << sqrt(distanceFromCenterToCenterSquared) << endl;
	else cout << "distanceFromCenterToCenterSquared is a negative number " << endl;
	cout << "cosAngleI = " << cosAngleI << ", an angle of " << acos(cosAngleI) * 57.3 << " degrees " << endl;
	cout << "angleToHome_ = " << angleToHome_ << endl;
	
	cout << "Home is " << distanceToHome_ << " meters away at an angle = " << angleToHome_ << endl;
	cout << "Bot's orientation with respect to the platform = " << botOrientation * 57.3 << " degrees " << endl;	
}

double getDistanceFromLeftToLeft() { return distanceFromLeftToLeft_; }
double getDistanceFromLeftToRight() { return distanceFromLeftToRight_; }
double getDistanceFromRightToLeft() { return distanceFromRightToLeft_; }
double getDistanceFromRightToRight() { return distanceFromRightToRight_; }
double getDistanceToHome() { return distanceToHome_; }
double getAngleToHome() { return angleToHome_; }


void getRadarRanges()
{
	ros::Time last_time;
   ros::Time current_time = ros::Time::now();
   
   distanceFromLeftToLeft_ = leftRanger_.getRange(LEFT_RADAR_NUMBER);
	last_time = ros::Time::now();
	while ( current_time.toSec() - last_time.toSec() < RADAR_WAIT_TIME) current_time = ros::Time::now(); // delay needed for radars to get data

	distanceFromLeftToRight_ = leftRanger_.getRange(RIGHT_RADAR_NUMBER);
	last_time = ros::Time::now();
	while ( current_time.toSec() - last_time.toSec() < RADAR_WAIT_TIME) current_time = ros::Time::now(); 
	
	distanceFromRightToLeft_ = rightRanger_.getRange(LEFT_RADAR_NUMBER);
	last_time = ros::Time::now();
	while ( current_time.toSec() - last_time.toSec() < RADAR_WAIT_TIME) current_time = ros::Time::now(); 
	
	distanceFromRightToRight_ = rightRanger_.getRange(RIGHT_RADAR_NUMBER);
	last_time = ros::Time::now();
	while ( current_time.toSec() - last_time.toSec() < RADAR_WAIT_TIME) current_time = ros::Time::now();
}

void testDataRadarRanges()
{  
   /*
   // 2 meters away, facing the robot, with radar seps both = 1000 (have to set that in the #defines)
   distanceFromLeftToLeft_ = 2000.;
	distanceFromLeftToRight_ = 2236.;
	distanceFromRightToLeft_ = 2236.;
	distanceFromRightToRight_ = 2000.;
	*/
	
	distanceFromLeftToLeft_ = 2393.;
	distanceFromLeftToRight_ = 2335.;
	distanceFromRightToLeft_ = 2706.;
	distanceFromRightToRight_ = 2155.;
	
	
}
	
};

int main(int argc, char** argv)
{
{'fraction_non_alphanumeric': 0.1175857706333259, 'fraction_numerical': 0.017340180099724642, 'mean_word_length': 5.320790216368768, 'pattern_counts': {'":': 0, '<': 294, '<?xml version=': 0, '>': 12, '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': '3583996', 'n_tokens_mistral': 4968, 'n_tokens_neox': 4473, 'n_words': 843}