int main (int argc, char* argv[]) {
int status;
Ice::CommunicatorPtr ic;
try {
ic = Ice::initialize(argc, argv);
readArgs(&argc, argv);
// Contact to Encoders
Ice::ObjectPrx Encoders = ic->propertyToProxy("automata.Encoders.Proxy");
if (Encoders == 0)
throw "Could not create proxy with Encoders";
Encodersprx = jderobot::EncodersPrx::checkedCast(Encoders);
if (Encodersprx == 0)
throw "Invalid proxy automata.Encoders.Proxy";
std::cout << "Encoders connected" << std::endl;
// Contact to Motors
Ice::ObjectPrx Motors = ic->propertyToProxy("automata.Motors.Proxy");
if (Motors == 0)
throw "Could not create proxy with Motors";
Motorsprx = jderobot::MotorsPrx::checkedCast(Motors);
if (Motorsprx == 0)
throw "Invalid proxy automata.Motors.Proxy";
std::cout << "Motors connected" << std::endl;
if (displayGui){
automatagui = new AutomataGui(argc, argv);
displayGui = showAutomataGui();
}
pthread_create(&thr_sub_1, NULL, &subautomata_1, NULL);
pthread_create(&thr_sub_2, NULL, &subautomata_2, NULL);
pthread_create(&thr_sub_3, NULL, &subautomata_3, NULL);
pthread_join(thr_sub_1, NULL);
pthread_join(thr_sub_2, NULL);
pthread_join(thr_sub_3, NULL);
if (displayGui)
pthread_join(thr_automatagui, NULL);
} catch ( const Ice::Exception& ex ) {
std::cerr << ex << std::endl;
status = 1;
} catch ( const char* msg ) {
std::cerr << msg << std::endl;
status = 1;
}
if (ic)
ic->destroy();
return status;
}
pointcloudClient::pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix) {
// TODO Auto-generated constructor stub
this->prefix=prefix;
Ice::PropertiesPtr prop;
prop = ic->getProperties();
this->refreshRate=0;
int fps=prop->getPropertyAsIntWithDefault(prefix+"Fps",10);
this->cycle=(float)(1/(float)fps)*1000000;
try{
Ice::ObjectPrx basePointCloud = ic->propertyToProxy(prefix+"Proxy");
if (0==basePointCloud){
throw prefix + " Could not create proxy";
}
else {
this->prx = jderobot::pointCloudPrx::checkedCast(basePointCloud);
if (0==this->prx)
throw "Invalid proxy" + prefix;
}
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
throw "Invalid proxy" + prefix;
}
catch (const char* msg) {
std::cerr << msg << std::endl;
jderobot::Logger::getInstance()->error(prefix + " Not camera provided");
throw "Invalid proxy" + prefix;
}
_done=false;
this->pauseStatus=false;
this->newData=false;
}
laserClient::laserClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug) {
// TODO Auto-generated constructor stub
this->prefix=prefix;
this->debug= debug;
Ice::PropertiesPtr prop;
prop = ic->getProperties();
int fps=prop->getPropertyAsIntWithDefault(prefix+"Fps",10);
this->cycle=(float)(1/(float)fps)*1000000;
try{
Ice::ObjectPrx basePointCloud = ic->propertyToProxy(prefix+"Proxy");
if (0==basePointCloud){
throw prefix + " Could not create proxy";
}
else {
this->prx = jderobot::LaserPrx::checkedCast(basePointCloud);
if (0==this->prx)
throw "Invalid proxy" + prefix;
}
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
}
catch (const char* msg) {
std::cerr << msg << std::endl;
std::cout << prefix + " Not laser provided" << std::endl;
}
_done=false;
}
Actuators::Actuators(Ice::CommunicatorPtr ic)
{
this->ic = ic;
// Contact to MOTORS interface
Ice::ObjectPrx baseMotors = ic->propertyToProxy("introrob.Motors.Proxy");
if (0 == baseMotors){
motorsON = false;
std::cout << "Motors configuration not specified" <<std::endl;
//throw "Could not create proxy with motors";
}else{
// Cast to motors
try{
mprx = jderobot::MotorsPrx::checkedCast(baseMotors);
if (0 == mprx)
throw "Invalid proxy introrob.Motors.Proxy";
motorsON = true;
std::cout << "Motors connected" << std::endl;
}catch (Ice::ConnectionRefusedException& e){
motorsON=false;
std::cout << "Motors inactive" << std::endl;
}
}
motorVout= 0;
motorWout = 0;
motorLout= 0;
}
DetectorPrx initIceConnection(const std::string& detectorNameStr,
Ice::Identity& det_cb,
DetectorCallbackHandlerPtr cr)
{
initIce(detectorNameStr);
Ice::PropertiesPtr props = iceComm->getProperties();
std::string proxStr = detectorNameStr + ".Proxy";
DetectorPrx detector = NULL;
try
{
detector = DetectorPrx::checkedCast(
iceComm->propertyToProxy(proxStr)->ice_twoway());
}
catch (const IceUtil::NullHandleException& e)
{
localAndClientMsg( VLogger::ERROR, NULL, "Invalid proxy: '%s'. %s\n",
detectorNameStr.c_str(), e.what());
return NULL;
}
Ice::ObjectAdapterPtr adapter = iceComm->createObjectAdapter("");
det_cb.name = IceUtil::generateUUID();
det_cb.category = "";
adapter->add(cr, det_cb);
adapter->activate();
detector->ice_getConnection()->setAdapter(adapter);
// note that we need an ObjectAdapter to permit bidirectional communication
// if we want to get past firewalls without Glacier2
return detector; // Success
}
int main(int argc, char** argv){
int status;
cameraview::Viewer viewer;
Ice::CommunicatorPtr ic;
jderobot::cameraClient* camRGB;
try{
ic = EasyIce::initialize(argc,argv);
Ice::ObjectPrx base = ic->propertyToProxy("Cameraview.Camera.Proxy");
Ice::PropertiesPtr prop = ic->getProperties();
if (0==base)
throw "Could not create proxy";
camRGB = new jderobot::cameraClient(ic,"Cameraview.Camera.");
if (camRGB == NULL){
throw "Invalid proxy";
}
camRGB->start();
cv::Mat rgb;
while(viewer.isVisible()){
//jderobot::ImageDataPtr data = camRGB->getImageData(format);
camRGB->getImage(rgb);
viewer.display(rgb);
viewer.displayFrameRate(camRGB->getRefreshRate());
}
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
status = 1;
} catch (const char* msg) {
std::cerr << msg << std::endl;
status = 1;
}
if (ic)
ic->destroy();
camRGB->stop_thread();
delete(camRGB);
return status;
}
int main(int argc, char** argv){
int status;
Ice::CommunicatorPtr ic;
try{
ic = Ice::initialize(argc,argv);
std::string topicName = ic->getProperties()->getProperty("Cameraview_icestorm.Camera.TopicName");
std::cout << "Trying to conect to: " << topicName << std::endl;
Ice::ObjectPrx obj=ic->propertyToProxy("Cameraview_icestorm.Camera.TopicManager");
IceStorm::TopicManagerPrx topicManager=IceStorm::TopicManagerPrx::checkedCast(obj);
std::string objAdapterEndpoint = ic->getProperties()->getProperty("Cameraview_icestorm.Camera.ObjectAdapter");
Ice::ObjectAdapterPtr adapter=ic->createObjectAdapterWithEndpoints("CameraAdapter",objAdapterEndpoint);
ImageConsumerI* imageConsumer = new ImageConsumerI;
Ice::ObjectPrx proxy = adapter->addWithUUID(imageConsumer)->ice_oneway();
IceStorm::TopicPrx topic;
try {
topic = topicManager->retrieve(topicName);
IceStorm::QoS qos;
topic->subscribeAndGetPublisher(qos, proxy);
}
catch (const IceStorm::NoSuchTopic& ex) {
std::cerr << ex << std::endl;
}
adapter->activate();
ic->waitForShutdown();
topic->unsubscribe(proxy);
if (ic)
ic->destroy();
return status;
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
status = 1;
} catch (const char* msg) {
std::cerr << msg << std::endl;
status = 1;
}
}
int main(int argc, char** argv) {
int status;
cameraview::colorTuner viewer;
Ice::CommunicatorPtr ic;
try {
ic = EasyIce::initialize(argc,argv);
Ice::ObjectPrx base = ic->propertyToProxy("Cameraview.Camera.Proxy");
if (0==base)
throw "Could not create proxy";
/*cast to CameraPrx*/
jderobot::CameraPrx cprx = jderobot::CameraPrx::checkedCast(base);
if (0==cprx)
throw "Invalid proxy";
while(viewer.isVisible()) {
jderobot::ImageDataPtr data = cprx->getImageData(colorspaces::ImageRGB8::FORMAT_RGB8.get()->name);
cv::Mat image = cv::Mat(cv::Size(data->description->width,data->description->height),CV_8UC3,&(data->pixelData[0]));
colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(data->description->format);
if (!fmt)
throw "Format not supported";
viewer.display(image);
image.release();
}
} catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
status = 1;
} catch (const char* msg) {
std::cerr << msg << std::endl;
status = 1;
}
if (ic)
ic->destroy();
return status;
}
int main(int argc, char **argv) {
Ice::CommunicatorPtr ic;
jderobot::cameraClient* camRGB;
try {
ic = EasyIce::initialize(argc, argv);
Ice::ObjectPrx base = ic->propertyToProxy("Cameraview.Camera.Proxy");
Ice::PropertiesPtr prop = ic->getProperties();
if (0==base)
throw "Could not create Proxy\n";
camRGB = new jderobot::cameraClient(ic, "Cameraview.Camera.");
if (camRGB == NULL) {
throw "Invalid Proxy";
} else {
camRGB->start();
std::cout << "Using ICE camera server..";
cv::Mat rgb;
while(1) {
camRGB->getImage(rgb);
if (rgb.rows==0 || rgb.cols==0) continue;
imshow("frame:ICE", rgb);
waitKey(33);
}
}
} catch (const char* msg) {
}
ros::init(argc, argv, "listener");
ros::NodeHandle n,nh;
image_transport::ImageTransport it(nh);
image_transport::Subscriber camera_sub = it.subscribe("cameratopic", 1000, cameracallback);
ros::Subscriber sub = n.subscribe("chatter", 1001, chatterCallback);
ros::spin();
return 0;
}
Pose3dIceClient::Pose3dIceClient(Ice::CommunicatorPtr ic, std::string prefix) {
this->prefix=prefix;
Ice::PropertiesPtr prop;
prop = ic->getProperties();
int fps=prop->getPropertyAsIntWithDefault(prefix+".Fps",30);
this->cycle=(float)(1/(float)fps)*1000000;
Ice::ObjectPrx basePose = ic->propertyToProxy(prefix+".Proxy");
if (0==basePose){
this->on = false;
std::cout << prefix + ".Proxy configuration not specified" <<std::endl;
}
else {
try{
this->prx = jderobot::Pose3DPrx::checkedCast(basePose);
if (0 == this->prx){
this->on = false;
std::cout <<"Invalid proxy "+ prefix + ".Proxy" <<std::endl;
}else{
this->on = true;
std::cout << prefix + " connected" << std::endl;
}
}catch (const Ice::ConnectionRefusedException& e) {
std::cout << prefix +" inactive" << std::endl;
}
catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
}
}
this->pauseStatus=false;
}
cameraClient::cameraClient(Ice::CommunicatorPtr ic, std::string prefix) {
this->prefix=prefix;
Ice::PropertiesPtr prop;
prop = ic->getProperties();
Ice::ObjectPrx baseCamera;
this->refreshRate=0;
this->mImageFormat.empty();
this->newData=false;
int fps=prop->getPropertyAsIntWithDefault(prefix+"Fps",30);
this->cycle=(float)(1/(float)fps)*1000000;
try{
baseCamera = ic->propertyToProxy(prefix+"Proxy");
if (0==baseCamera){
throw prefix + "Could not create proxy with Camera";
}
else {
this->prx= jderobot::CameraPrx::checkedCast(baseCamera);
if (0==this->prx)
throw "Invalid " + prefix + ".Proxy";
}
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
}
catch (const char* msg) {
std::cerr << msg << std::endl;
LOG(FATAL) << prefix + " Not camera provided";
}
//check if default format is defined
std::string definedFormat=prop->getProperty(prefix+"ImageFormat");
this->mImageFormat = CameraUtils::negotiateDefaultFormat(this->prx,definedFormat);
jderobot::ImageDataPtr data = this->prx->getImageData(this->mImageFormat);
this->size=cv::Size(data->description->width,data->description->height);
_done=false;
this->pauseStatus=false;
}
Control::Control(Ice::CommunicatorPtr ic, Shared* sm)
{
/*Obtaining the configuration file (*.cfg) properties such as ports and IP's*/
this->ic = ic;
this->sm = sm;
Ice::PropertiesPtr prop = ic->getProperties();
std::string cam = prop->getPropertyWithDefault("basic_component_qt.Camera1.Proxy", "miss");
if (!boost::iequals(cam , "miss"))
{
/*Creation of a proxy to connect with cameraServer*/
Ice::ObjectPrx base = ic->propertyToProxy("basic_component_qt.Camera1.Proxy");
if (0==base)
throw "Could not create proxy";
/*cast to CameraPrx*/
cprx = jderobot::CameraPrx::checkedCast(base);
if (0==cprx)
throw "Invalid proxy";
cameraOn = true;
/*Get the image data from the camera proxy*/
jderobot::ImageDataPtr data = cprx->getImageData();
/*Create the first image obtained from the camera and stores in the shared memory*/
this->sm->createImage(data);
}
else
{
cameraOn = false;
/*Create an empty image if there is no camera connected*/
this->sm->createEmptyImage();
std::cout << "No camera connected" << std::endl;
}
}
int
run(int argc, char* argv[], const Ice::CommunicatorPtr& communicator1, const Ice::CommunicatorPtr& communicator2)
{
const string proxyProperty = "Echo.Proxy";
EchoPrx echo1 = EchoPrx::checkedCast(communicator1->propertyToProxy(proxyProperty));
if(!echo1)
{
cerr << argv[0] << ": invalid proxy" << endl;
return EXIT_FAILURE;
}
EchoPrx echo2 = EchoPrx::checkedCast(communicator2->propertyToProxy(proxyProperty));
if(!echo2)
{
cerr << argv[0] << ": invalid proxy" << endl;
return EXIT_FAILURE;
}
menu();
string greeting = "Bonne journ\351e";
char c;
do
{
try
{
cout << "==> ";
cin >> c;
if(c == 't')
{
string ret = echo1->echoString(greeting);
cout << "Received (LATIN-1): \"" << IceUtil::escapeString(ret, "") << '\"' << endl;
}
else if(c == 'u')
{
string ret = echo2->echoString(greeting);
cout << "Received (LATIN-1): \"" << IceUtil::escapeString(ret, "") << '\"' << endl;
}
else if(c == 's')
{
echo1->shutdown();
}
else if(c == 'x')
{
// Nothing to do
}
else if(c == '?')
{
menu();
}
else
{
cout << "unknown command `" << c << "'" << endl;
menu();
}
}
catch(const Ice::Exception& ex)
{
cerr << ex << endl;
}
}
while(cin.good() && c != 'x');
return EXIT_SUCCESS;
}
Test::MyClassPrx
allTests(const Ice::CommunicatorPtr& communicator)
{
cout << "testing stringToProxy... " << flush;
string ref = "test:default -p 12010";
Ice::ObjectPrx base = communicator->stringToProxy(ref);
test(base);
Ice::ObjectPrx b1 = communicator->stringToProxy("test");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getAdapterId().empty() && b1->ice_getFacet().empty());
b1 = communicator->stringToProxy("test ");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy(" test ");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy(" test");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy("'test -f facet'");
test(b1->ice_getIdentity().name == "test -f facet" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
try
{
b1 = communicator->stringToProxy("\"test -f facet'");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("\"test -f facet\"");
test(b1->ice_getIdentity().name == "test -f facet" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy("\"test -f facet@test\"");
test(b1->ice_getIdentity().name == "test -f facet@test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy("\"test -f facet@test @test\"");
test(b1->ice_getIdentity().name == "test -f facet@test @test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
try
{
b1 = communicator->stringToProxy("test test");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("test\\040test");
test(b1->ice_getIdentity().name == "test test" && b1->ice_getIdentity().category.empty());
try
{
b1 = communicator->stringToProxy("test\\777");
test(false);
}
catch(const Ice::IdentityParseException&)
{
}
b1 = communicator->stringToProxy("test\\40test");
test(b1->ice_getIdentity().name == "test test");
// Test some octal and hex corner cases.
b1 = communicator->stringToProxy("test\\4test");
test(b1->ice_getIdentity().name == "test\4test");
b1 = communicator->stringToProxy("test\\04test");
test(b1->ice_getIdentity().name == "test\4test");
b1 = communicator->stringToProxy("test\\004test");
test(b1->ice_getIdentity().name == "test\4test");
b1 = communicator->stringToProxy("test\\1114test");
test(b1->ice_getIdentity().name == "test\1114test");
b1 = communicator->stringToProxy("test\\b\\f\\n\\r\\t\\'\\\"\\\\test");
test(b1->ice_getIdentity().name == "test\b\f\n\r\t\'\"\\test" && b1->ice_getIdentity().category.empty());
b1 = communicator->stringToProxy("category/test");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId().empty());
b1 = communicator->stringToProxy("");
test(!b1);
b1 = communicator->stringToProxy("\"\"");
test(!b1);
try
{
b1 = communicator->stringToProxy("\"\" test"); // Invalid trailing characters.
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
try
{
b1 = communicator->stringToProxy("test:"); // Missing endpoint.
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
b1 = communicator->stringToProxy("test@adapter");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getAdapterId() == "adapter");
try
{
b1 = communicator->stringToProxy("id@adapter test");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("category/test@adapter");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId() == "adapter");
b1 = communicator->stringToProxy("category/test@adapter:tcp");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId() == "adapter:tcp");
b1 = communicator->stringToProxy("'category 1/test'@adapter");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category 1" &&
b1->ice_getAdapterId() == "adapter");
b1 = communicator->stringToProxy("'category/test 1'@adapter");
test(b1->ice_getIdentity().name == "test 1" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId() == "adapter");
b1 = communicator->stringToProxy("'category/test'@'adapter 1'");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId() == "adapter 1");
b1 = communicator->stringToProxy("\"category \\/test@foo/test\"@adapter");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category /test@foo" &&
b1->ice_getAdapterId() == "adapter");
b1 = communicator->stringToProxy("\"category \\/test@foo/test\"@\"adapter:tcp\"");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category /test@foo" &&
b1->ice_getAdapterId() == "adapter:tcp");
b1 = communicator->stringToProxy("id -f facet");
test(b1->ice_getIdentity().name == "id" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet");
b1 = communicator->stringToProxy("id -f 'facet x'");
test(b1->ice_getIdentity().name == "id" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet x");
b1 = communicator->stringToProxy("id -f \"facet x\"");
test(b1->ice_getIdentity().name == "id" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet x");
try
{
b1 = communicator->stringToProxy("id -f \"facet x");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
try
{
b1 = communicator->stringToProxy("id -f \'facet x");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("test -f facet:tcp");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet" && b1->ice_getAdapterId().empty());
b1 = communicator->stringToProxy("test -f \"facet:tcp\"");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet:tcp" && b1->ice_getAdapterId().empty());
b1 = communicator->stringToProxy("test -f facet@test");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet" && b1->ice_getAdapterId() == "test");
b1 = communicator->stringToProxy("test -f 'facet@test'");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet@test" && b1->ice_getAdapterId().empty());
b1 = communicator->stringToProxy("test -f 'facet@test'@test");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet@test" && b1->ice_getAdapterId() == "test");
try
{
b1 = communicator->stringToProxy("test -f facet@test @test");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("test");
test(b1->ice_isTwoway());
b1 = communicator->stringToProxy("test -t");
test(b1->ice_isTwoway());
b1 = communicator->stringToProxy("test -o");
test(b1->ice_isOneway());
b1 = communicator->stringToProxy("test -O");
test(b1->ice_isBatchOneway());
b1 = communicator->stringToProxy("test -d");
test(b1->ice_isDatagram());
b1 = communicator->stringToProxy("test -D");
test(b1->ice_isBatchDatagram());
b1 = communicator->stringToProxy("test");
test(!b1->ice_isSecure());
b1 = communicator->stringToProxy("test -s");
test(b1->ice_isSecure());
test(b1->ice_getEncodingVersion() == Ice::currentEncoding);
b1 = communicator->stringToProxy("test -e 1.0");
test(b1->ice_getEncodingVersion().major == 1 && b1->ice_getEncodingVersion().minor == 0);
b1 = communicator->stringToProxy("test -e 6.5");
test(b1->ice_getEncodingVersion().major == 6 && b1->ice_getEncodingVersion().minor == 5);
b1 = communicator->stringToProxy("test -p 1.0 -e 1.0");
test(b1->ice_toString() == "test -t -e 1.0");
b1 = communicator->stringToProxy("test -p 6.5 -e 1.0");
test(b1->ice_toString() == "test -t -p 6.5 -e 1.0");
try
{
b1 = communicator->stringToProxy("test:tcp@adapterId");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
// This is an unknown endpoint warning, not a parse exception.
//
//try
//{
// b1 = communicator->stringToProxy("test -f the:facet:tcp");
// test(false);
//}
//catch(const Ice::EndpointParseException&)
//{
//}
try
{
b1 = communicator->stringToProxy("test::tcp");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
//
// Test for bug ICE-5543: escaped escapes in stringToIdentity
//
Ice::Identity id = { "test", ",X2QNUAzSBcJ_e$AV;E\\" };
Ice::Identity id2 = communicator->stringToIdentity(communicator->identityToString(id));
test(id == id2);
id.name = "test";
id.category = ",X2QNUAz\\SB\\/cJ_e$AV;E\\\\";
id2 = communicator->stringToIdentity(communicator->identityToString(id));
test(id == id2);
cout << "ok" << endl;
cout << "testing propertyToProxy... " << flush;
Ice::PropertiesPtr prop = communicator->getProperties();
string propertyPrefix = "Foo.Proxy";
prop->setProperty(propertyPrefix, "test:default -p 12010");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getAdapterId().empty() && b1->ice_getFacet().empty());
string property;
property = propertyPrefix + ".Locator";
test(!b1->ice_getLocator());
prop->setProperty(property, "locator:default -p 10000");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getLocator() && b1->ice_getLocator()->ice_getIdentity().name == "locator");
prop->setProperty(property, "");
property = propertyPrefix + ".LocatorCacheTimeout";
test(b1->ice_getLocatorCacheTimeout() == -1);
prop->setProperty(property, "1");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getLocatorCacheTimeout() == 1);
prop->setProperty(property, "");
// Now retest with an indirect proxy.
prop->setProperty(propertyPrefix, "test");
property = propertyPrefix + ".Locator";
prop->setProperty(property, "locator:default -p 10000");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getLocator() && b1->ice_getLocator()->ice_getIdentity().name == "locator");
prop->setProperty(property, "");
property = propertyPrefix + ".LocatorCacheTimeout";
test(b1->ice_getLocatorCacheTimeout() == -1);
prop->setProperty(property, "1");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getLocatorCacheTimeout() == 1);
prop->setProperty(property, "");
// This cannot be tested so easily because the property is cached
// on communicator initialization.
//
//prop->setProperty("Ice.Default.LocatorCacheTimeout", "60");
//b1 = communicator->propertyToProxy(propertyPrefix);
//test(b1->ice_getLocatorCacheTimeout() == 60);
//prop->setProperty("Ice.Default.LocatorCacheTimeout", "");
prop->setProperty(propertyPrefix, "test:default -p 12010");
property = propertyPrefix + ".Router";
test(!b1->ice_getRouter());
prop->setProperty(property, "router:default -p 10000");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getRouter() && b1->ice_getRouter()->ice_getIdentity().name == "router");
prop->setProperty(property, "");
property = propertyPrefix + ".PreferSecure";
test(!b1->ice_isPreferSecure());
prop->setProperty(property, "1");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_isPreferSecure());
prop->setProperty(property, "");
property = propertyPrefix + ".ConnectionCached";
test(b1->ice_isConnectionCached());
prop->setProperty(property, "0");
b1 = communicator->propertyToProxy(propertyPrefix);
test(!b1->ice_isConnectionCached());
prop->setProperty(property, "");
property = propertyPrefix + ".InvocationTimeout";
test(b1->ice_getInvocationTimeout() == -1);
prop->setProperty(property, "1000");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getInvocationTimeout() == 1000);
prop->setProperty(property, "");
property = propertyPrefix + ".EndpointSelection";
test(b1->ice_getEndpointSelection() == Ice::Random);
prop->setProperty(property, "Random");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getEndpointSelection() == Ice::Random);
prop->setProperty(property, "Ordered");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getEndpointSelection() == Ice::Ordered);
prop->setProperty(property, "");
property = propertyPrefix + ".CollocationOptimized";
test(b1->ice_isCollocationOptimized());
prop->setProperty(property, "0");
b1 = communicator->propertyToProxy(propertyPrefix);
test(!b1->ice_isCollocationOptimized());
prop->setProperty(property, "");
property = propertyPrefix + ".Context.c1";
test(b1->ice_getContext()["c1"].empty());
prop->setProperty(property, "TEST");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getContext()["c1"] == "TEST");
property = propertyPrefix + ".Context.c2";
test(b1->ice_getContext()["c2"].empty());
prop->setProperty(property, "TEST");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getContext()["c2"] == "TEST");
prop->setProperty(propertyPrefix + ".Context.c1", "");
prop->setProperty(propertyPrefix + ".Context.c2", "");
cout << "ok" << endl;
cout << "testing proxyToProperty... " << flush;
b1 = communicator->stringToProxy("test");
b1 = b1->ice_collocationOptimized(true);
b1 = b1->ice_connectionCached(true);
b1 = b1->ice_preferSecure(false);
b1 = b1->ice_endpointSelection(Ice::Ordered);
b1 = b1->ice_locatorCacheTimeout(100);
b1 = b1->ice_invocationTimeout(1234);
Ice::EncodingVersion v = { 1, 0 };
b1 = b1->ice_encodingVersion(v);
Ice::ObjectPrx router = communicator->stringToProxy("router");
router = router->ice_collocationOptimized(false);
router = router->ice_connectionCached(true);
router = router->ice_preferSecure(true);
router = router->ice_endpointSelection(Ice::Random);
router = router->ice_locatorCacheTimeout(200);
router = router->ice_invocationTimeout(1500);
Ice::ObjectPrx locator = communicator->stringToProxy("locator");
locator = locator->ice_collocationOptimized(true);
locator = locator->ice_connectionCached(false);
locator = locator->ice_preferSecure(true);
locator = locator->ice_endpointSelection(Ice::Random);
locator = locator->ice_locatorCacheTimeout(300);
locator = locator->ice_invocationTimeout(1500);
locator = locator->ice_router(Ice::RouterPrx::uncheckedCast(router));
b1 = b1->ice_locator(Ice::LocatorPrx::uncheckedCast(locator));
Ice::PropertyDict proxyProps = communicator->proxyToProperty(b1, "Test");
test(proxyProps.size() == 21);
test(proxyProps["Test"] == "test -t -e 1.0");
test(proxyProps["Test.CollocationOptimized"] == "1");
test(proxyProps["Test.ConnectionCached"] == "1");
test(proxyProps["Test.PreferSecure"] == "0");
test(proxyProps["Test.EndpointSelection"] == "Ordered");
test(proxyProps["Test.LocatorCacheTimeout"] == "100");
test(proxyProps["Test.InvocationTimeout"] == "1234");
test(proxyProps["Test.Locator"] == "locator -t -e " + Ice::encodingVersionToString(Ice::currentEncoding));
// Locator collocation optimization is always disabled.
//test(proxyProps["Test.Locator.CollocationOptimized"] == "1");
test(proxyProps["Test.Locator.ConnectionCached"] == "0");
test(proxyProps["Test.Locator.PreferSecure"] == "1");
test(proxyProps["Test.Locator.EndpointSelection"] == "Random");
test(proxyProps["Test.Locator.LocatorCacheTimeout"] == "300");
test(proxyProps["Test.Locator.InvocationTimeout"] == "1500");
test(proxyProps["Test.Locator.Router"] == "router -t -e " + Ice::encodingVersionToString(Ice::currentEncoding));
test(proxyProps["Test.Locator.Router.CollocationOptimized"] == "0");
test(proxyProps["Test.Locator.Router.ConnectionCached"] == "1");
test(proxyProps["Test.Locator.Router.PreferSecure"] == "1");
test(proxyProps["Test.Locator.Router.EndpointSelection"] == "Random");
test(proxyProps["Test.Locator.Router.LocatorCacheTimeout"] == "200");
test(proxyProps["Test.Locator.Router.InvocationTimeout"] == "1500");
cout << "ok" << endl;
cout << "testing ice_getCommunicator... " << flush;
test(base->ice_getCommunicator() == communicator);
cout << "ok" << endl;
cout << "testing proxy methods... " << flush;
test(communicator->identityToString(base->ice_identity(communicator->stringToIdentity("other"))->ice_getIdentity())
== "other");
test(base->ice_facet("facet")->ice_getFacet() == "facet");
test(base->ice_adapterId("id")->ice_getAdapterId() == "id");
test(base->ice_twoway()->ice_isTwoway());
test(base->ice_oneway()->ice_isOneway());
test(base->ice_batchOneway()->ice_isBatchOneway());
test(base->ice_datagram()->ice_isDatagram());
test(base->ice_batchDatagram()->ice_isBatchDatagram());
test(base->ice_secure(true)->ice_isSecure());
test(!base->ice_secure(false)->ice_isSecure());
test(base->ice_collocationOptimized(true)->ice_isCollocationOptimized());
test(!base->ice_collocationOptimized(false)->ice_isCollocationOptimized());
test(base->ice_preferSecure(true)->ice_isPreferSecure());
test(!base->ice_preferSecure(false)->ice_isPreferSecure());
test(base->ice_encodingVersion(Ice::Encoding_1_0)->ice_getEncodingVersion() == Ice::Encoding_1_0);
test(base->ice_encodingVersion(Ice::Encoding_1_1)->ice_getEncodingVersion() == Ice::Encoding_1_1);
test(base->ice_encodingVersion(Ice::Encoding_1_0)->ice_getEncodingVersion() != Ice::Encoding_1_1);
try
{
base->ice_timeout(0);
test(false);
}
catch(const IceUtil::IllegalArgumentException&)
{
}
try
{
base->ice_timeout(-1);
}
catch(const IceUtil::IllegalArgumentException&)
{
test(false);
}
try
{
base->ice_timeout(-2);
test(false);
}
catch(const IceUtil::IllegalArgumentException&)
{
}
try
{
base->ice_invocationTimeout(0);
test(false);
}
catch(const IceUtil::IllegalArgumentException&)
{
}
try
{
base->ice_invocationTimeout(-1);
base->ice_invocationTimeout(-2);
}
catch(const IceUtil::IllegalArgumentException&)
{
test(false);
}
try
{
base->ice_invocationTimeout(-3);
test(false);
}
catch(const IceUtil::IllegalArgumentException&)
{
}
try
{
base->ice_locatorCacheTimeout(0);
}
catch(const IceUtil::IllegalArgumentException&)
{
test(false);
}
try
{
base->ice_locatorCacheTimeout(-1);
}
catch(const IceUtil::IllegalArgumentException&)
{
test(false);
}
try
{
base->ice_locatorCacheTimeout(-2);
test(false);
}
catch(const IceUtil::IllegalArgumentException&)
{
}
cout << "ok" << endl;
cout << "testing proxy comparison... " << flush;
test(communicator->stringToProxy("foo") == communicator->stringToProxy("foo"));
test(communicator->stringToProxy("foo") != communicator->stringToProxy("foo2"));
test(communicator->stringToProxy("foo") < communicator->stringToProxy("foo2"));
test(!(communicator->stringToProxy("foo2") < communicator->stringToProxy("foo")));
Ice::ObjectPrx compObj = communicator->stringToProxy("foo");
test(compObj->ice_facet("facet") == compObj->ice_facet("facet"));
test(compObj->ice_facet("facet") != compObj->ice_facet("facet1"));
test(compObj->ice_facet("facet") < compObj->ice_facet("facet1"));
test(!(compObj->ice_facet("facet") < compObj->ice_facet("facet")));
test(compObj->ice_oneway() == compObj->ice_oneway());
test(compObj->ice_oneway() != compObj->ice_twoway());
test(compObj->ice_twoway() < compObj->ice_oneway());
test(!(compObj->ice_oneway() < compObj->ice_twoway()));
test(compObj->ice_secure(true) == compObj->ice_secure(true));
test(compObj->ice_secure(false) != compObj->ice_secure(true));
test(compObj->ice_secure(false) < compObj->ice_secure(true));
test(!(compObj->ice_secure(true) < compObj->ice_secure(false)));
test(compObj->ice_collocationOptimized(true) == compObj->ice_collocationOptimized(true));
test(compObj->ice_collocationOptimized(false) != compObj->ice_collocationOptimized(true));
test(compObj->ice_collocationOptimized(false) < compObj->ice_collocationOptimized(true));
test(!(compObj->ice_collocationOptimized(true) < compObj->ice_collocationOptimized(false)));
test(compObj->ice_connectionCached(true) == compObj->ice_connectionCached(true));
test(compObj->ice_connectionCached(false) != compObj->ice_connectionCached(true));
test(compObj->ice_connectionCached(false) < compObj->ice_connectionCached(true));
test(!(compObj->ice_connectionCached(true) < compObj->ice_connectionCached(false)));
test(compObj->ice_endpointSelection(Ice::Random) == compObj->ice_endpointSelection(Ice::Random));
test(compObj->ice_endpointSelection(Ice::Random) != compObj->ice_endpointSelection(Ice::Ordered));
test(compObj->ice_endpointSelection(Ice::Random) < compObj->ice_endpointSelection(Ice::Ordered));
test(!(compObj->ice_endpointSelection(Ice::Ordered) < compObj->ice_endpointSelection(Ice::Random)));
test(compObj->ice_connectionId("id2") == compObj->ice_connectionId("id2"));
test(compObj->ice_connectionId("id1") != compObj->ice_connectionId("id2"));
test(compObj->ice_connectionId("id1") < compObj->ice_connectionId("id2"));
test(!(compObj->ice_connectionId("id2") < compObj->ice_connectionId("id1")));
test(compObj->ice_connectionId("id1")->ice_getConnectionId() == "id1");
test(compObj->ice_connectionId("id2")->ice_getConnectionId() == "id2");
test(compObj->ice_compress(true) == compObj->ice_compress(true));
test(compObj->ice_compress(false) != compObj->ice_compress(true));
test(compObj->ice_compress(false) < compObj->ice_compress(true));
test(!(compObj->ice_compress(true) < compObj->ice_compress(false)));
test(compObj->ice_timeout(20) == compObj->ice_timeout(20));
test(compObj->ice_timeout(10) != compObj->ice_timeout(20));
test(compObj->ice_timeout(10) < compObj->ice_timeout(20));
test(!(compObj->ice_timeout(20) < compObj->ice_timeout(10)));
Ice::LocatorPrx loc1 = Ice::LocatorPrx::uncheckedCast(communicator->stringToProxy("loc1:default -p 10000"));
Ice::LocatorPrx loc2 = Ice::LocatorPrx::uncheckedCast(communicator->stringToProxy("loc2:default -p 10000"));
test(compObj->ice_locator(0) == compObj->ice_locator(0));
test(compObj->ice_locator(loc1) == compObj->ice_locator(loc1));
test(compObj->ice_locator(loc1) != compObj->ice_locator(0));
test(compObj->ice_locator(0) != compObj->ice_locator(loc2));
test(compObj->ice_locator(loc1) != compObj->ice_locator(loc2));
test(compObj->ice_locator(0) < compObj->ice_locator(loc1));
test(!(compObj->ice_locator(loc1) < compObj->ice_locator(0)));
test(compObj->ice_locator(loc1) < compObj->ice_locator(loc2));
test(!(compObj->ice_locator(loc2) < compObj->ice_locator(loc1)));
Ice::RouterPrx rtr1 = Ice::RouterPrx::uncheckedCast(communicator->stringToProxy("rtr1:default -p 10000"));
Ice::RouterPrx rtr2 = Ice::RouterPrx::uncheckedCast(communicator->stringToProxy("rtr2:default -p 10000"));
test(compObj->ice_router(0) == compObj->ice_router(0));
test(compObj->ice_router(rtr1) == compObj->ice_router(rtr1));
test(compObj->ice_router(rtr1) != compObj->ice_router(0));
test(compObj->ice_router(0) != compObj->ice_router(rtr2));
test(compObj->ice_router(rtr1) != compObj->ice_router(rtr2));
test(compObj->ice_router(0) < compObj->ice_router(rtr1));
test(!(compObj->ice_router(rtr1) < compObj->ice_router(0)));
test(compObj->ice_router(rtr1) < compObj->ice_router(rtr2));
test(!(compObj->ice_router(rtr2) < compObj->ice_router(rtr1)));
Ice::Context ctx1;
ctx1["ctx1"] = "v1";
Ice::Context ctx2;
ctx2["ctx2"] = "v2";
test(compObj->ice_context(Ice::Context()) == compObj->ice_context(Ice::Context()));
test(compObj->ice_context(ctx1) == compObj->ice_context(ctx1));
test(compObj->ice_context(ctx1) != compObj->ice_context(Ice::Context()));
test(compObj->ice_context(Ice::Context()) != compObj->ice_context(ctx2));
test(compObj->ice_context(ctx1) != compObj->ice_context(ctx2));
test(compObj->ice_context(ctx1) < compObj->ice_context(ctx2));
test(!(compObj->ice_context(ctx2) < compObj->ice_context(ctx1)));
test(compObj->ice_preferSecure(true) == compObj->ice_preferSecure(true));
test(compObj->ice_preferSecure(true) != compObj->ice_preferSecure(false));
test(compObj->ice_preferSecure(false) < compObj->ice_preferSecure(true));
test(!(compObj->ice_preferSecure(true) < compObj->ice_preferSecure(false)));
Ice::ObjectPrx compObj1 = communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 10000");
Ice::ObjectPrx compObj2 = communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 10001");
test(compObj1 != compObj2);
test(compObj1 < compObj2);
test(!(compObj2 < compObj1));
compObj1 = communicator->stringToProxy("foo@MyAdapter1");
compObj2 = communicator->stringToProxy("foo@MyAdapter2");
test(compObj1 != compObj2);
test(compObj1 < compObj2);
test(!(compObj2 < compObj1));
test(compObj1->ice_locatorCacheTimeout(20) == compObj1->ice_locatorCacheTimeout(20));
test(compObj1->ice_locatorCacheTimeout(10) != compObj1->ice_locatorCacheTimeout(20));
test(compObj1->ice_locatorCacheTimeout(10) < compObj1->ice_locatorCacheTimeout(20));
test(!(compObj1->ice_locatorCacheTimeout(20) < compObj1->ice_locatorCacheTimeout(10)));
test(compObj1->ice_invocationTimeout(20) == compObj1->ice_invocationTimeout(20));
test(compObj1->ice_invocationTimeout(10) != compObj1->ice_invocationTimeout(20));
test(compObj1->ice_invocationTimeout(10) < compObj1->ice_invocationTimeout(20));
test(!(compObj1->ice_invocationTimeout(20) < compObj1->ice_invocationTimeout(10)));
compObj1 = communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 1000");
compObj2 = communicator->stringToProxy("foo@MyAdapter1");
test(compObj1 != compObj2);
test(compObj1 < compObj2);
test(!(compObj2 < compObj1));
Ice::EndpointSeq endpts1 = communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 10000")->ice_getEndpoints();
Ice::EndpointSeq endpts2 = communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 10001")->ice_getEndpoints();
test(endpts1 != endpts2);
test(endpts1 < endpts2);
test(!(endpts2 < endpts1));
test(endpts1 == communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 10000")->ice_getEndpoints());
test(compObj1->ice_encodingVersion(Ice::Encoding_1_0) == compObj1->ice_encodingVersion(Ice::Encoding_1_0));
test(compObj1->ice_encodingVersion(Ice::Encoding_1_0) != compObj1->ice_encodingVersion(Ice::Encoding_1_1));
test(compObj->ice_encodingVersion(Ice::Encoding_1_0) < compObj->ice_encodingVersion(Ice::Encoding_1_1));
test(!(compObj->ice_encodingVersion(Ice::Encoding_1_1) < compObj->ice_encodingVersion(Ice::Encoding_1_0)));
//
// TODO: Ideally we should also test comparison of fixed proxies.
//
cout << "ok" << endl;
cout << "testing checked cast... " << flush;
Test::MyClassPrx cl = Test::MyClassPrx::checkedCast(base);
test(cl);
Test::MyDerivedClassPrx derived = Test::MyDerivedClassPrx::checkedCast(cl);
test(derived);
test(cl == base);
test(derived == base);
test(cl == derived);
Ice::LocatorPrx loc = Ice::LocatorPrx::checkedCast(base);
test(loc == 0);
//
// Upcasting
//
Test::MyClassPrx cl2 = Test::MyClassPrx::checkedCast(derived);
Ice::ObjectPrx obj = Ice::ObjectPrx::checkedCast(derived);
test(cl2);
test(obj);
test(cl2 == obj);
test(cl2 == derived);
//
// Now with alternate API
//
cl = checkedCast<Test::MyClassPrx>(base);
test(cl);
derived = checkedCast<Test::MyDerivedClassPrx>(cl);
test(derived);
test(cl == base);
test(derived == base);
test(cl == derived);
loc = checkedCast<Ice::LocatorPrx>(base);
test(loc == 0);
cl2 = checkedCast<Test::MyClassPrx>(derived);
obj = checkedCast<Ice::ObjectPrx>(derived);
test(cl2);
test(obj);
test(cl2 == obj);
test(cl2 == derived);
cout << "ok" << endl;
cout << "testing checked cast with context... " << flush;
Ice::Context c = cl->getContext();
test(c.size() == 0);
c["one"] = "hello";
c["two"] = "world";
cl = Test::MyClassPrx::checkedCast(base, c);
Ice::Context c2 = cl->getContext();
test(c == c2);
//
// Now with alternate API
//
cl = checkedCast<Test::MyClassPrx>(base);
c = cl->getContext();
test(c.size() == 0);
cl = checkedCast<Test::MyClassPrx>(base, c);
c2 = cl->getContext();
test(c == c2);
cout << "ok" << endl;
cout << "testing encoding versioning... " << flush;
string ref20 = "test -e 2.0:default -p 12010";
Test::MyClassPrx cl20 = Test::MyClassPrx::uncheckedCast(communicator->stringToProxy(ref20));
try
{
cl20->ice_ping();
test(false);
}
catch(const Ice::UnsupportedEncodingException&)
{
// Server 2.0 endpoint doesn't support 1.1 version.
}
string ref10 = "test -e 1.0:default -p 12010";
Test::MyClassPrx cl10 = Test::MyClassPrx::uncheckedCast(communicator->stringToProxy(ref10));
cl10->ice_ping();
cl10->ice_encodingVersion(Ice::Encoding_1_0)->ice_ping();
cl->ice_encodingVersion(Ice::Encoding_1_0)->ice_ping();
// 1.3 isn't supported but since a 1.3 proxy supports 1.1, the
// call will use the 1.1 encoding
string ref13 = "test -e 1.3:default -p 12010";
Test::MyClassPrx cl13 = Test::MyClassPrx::uncheckedCast(communicator->stringToProxy(ref13));
cl13->ice_ping();
cl13->end_ice_ping(cl13->begin_ice_ping());
try
{
// Send request with bogus 1.2 encoding.
Ice::EncodingVersion version = { 1, 2 };
Ice::OutputStreamPtr out = Ice::createOutputStream(communicator);
out->startEncapsulation();
out->endEncapsulation();
vector<Ice::Byte> inEncaps;
out->finished(inEncaps);
inEncaps[4] = version.major;
inEncaps[5] = version.minor;
vector<Ice::Byte> outEncaps;
cl->ice_invoke("ice_ping", Ice::Normal, inEncaps, outEncaps);
test(false);
}
catch(const Ice::UnknownLocalException& ex)
{
// The server thrown an UnsupportedEncodingException
test(ex.unknown.find("UnsupportedEncodingException") != string::npos);
}
try
{
// Send request with bogus 2.0 encoding.
Ice::EncodingVersion version = { 2, 0 };
Ice::OutputStreamPtr out = Ice::createOutputStream(communicator);
out->startEncapsulation();
out->endEncapsulation();
vector<Ice::Byte> inEncaps;
out->finished(inEncaps);
inEncaps[4] = version.major;
inEncaps[5] = version.minor;
vector<Ice::Byte> outEncaps;
cl->ice_invoke("ice_ping", Ice::Normal, inEncaps, outEncaps);
test(false);
}
catch(const Ice::UnknownLocalException& ex)
{
// The server thrown an UnsupportedEncodingException
test(ex.unknown.find("UnsupportedEncodingException") != string::npos);
}
cout << "ok" << endl;
cout << "testing protocol versioning... " << flush;
ref20 = "test -p 2.0:default -p 12010";
cl20 = Test::MyClassPrx::uncheckedCast(communicator->stringToProxy(ref20));
try
{
cl20->ice_ping();
test(false);
}
catch(const Ice::UnsupportedProtocolException&)
{
// Server 2.0 proxy doesn't support 1.0 version.
}
ref10 = "test -p 1.0:default -p 12010";
cl10 = Test::MyClassPrx::uncheckedCast(communicator->stringToProxy(ref10));
cl10->ice_ping();
// 1.3 isn't supported but since a 1.3 proxy supports 1.0, the
// call will use the 1.0 encoding
ref13 = "test -p 1.3:default -p 12010";
cl13 = Test::MyClassPrx::uncheckedCast(communicator->stringToProxy(ref13));
cl13->ice_ping();
cl13->end_ice_ping(cl13->begin_ice_ping());
cout << "ok" <<endl;
cout << "testing opaque endpoints... " << flush;
try
{
// Invalid -x option
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 99 -v abc -x abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing -t and -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Repeated -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 1 -t 1 -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Repeated -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 1 -v abc -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 1");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing arg for -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing arg for -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 1 -v");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Not a number for -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t x -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// < 0 for -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t -1 -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Invalid char for -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 99 -v x?c");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
// Legal TCP endpoint expressed as opaque endpoint
Ice::ObjectPrx p1 = communicator->stringToProxy("test -e 1.1:opaque -e 1.0 -t 1 -v CTEyNy4wLjAuMeouAAAQJwAAAA==");
string pstr = communicator->proxyToString(p1);
test(pstr == "test -t -e 1.1:tcp -h 127.0.0.1 -p 12010 -t 10000");
// Opaque endpoint encoded with 1.1 encoding.
Ice::ObjectPrx p2 = communicator->stringToProxy("test -e 1.1:opaque -e 1.1 -t 1 -v CTEyNy4wLjAuMeouAAAQJwAAAA==");
test(communicator->proxyToString(p2) == "test -t -e 1.1:tcp -h 127.0.0.1 -p 12010 -t 10000");
if(communicator->getProperties()->getPropertyAsInt("Ice.IPv6") == 0)
{
// Working?
#ifndef ICE_OS_WINRT
const bool ssl = communicator->getProperties()->getProperty("Ice.Default.Protocol") == "ssl";
#else
const bool ssl = true;
#endif
const bool tcp = communicator->getProperties()->getProperty("Ice.Default.Protocol") == "tcp";
if(tcp)
{
p1->ice_encodingVersion(Ice::Encoding_1_0)->ice_ping();
}
// Two legal TCP endpoints expressed as opaque endpoints
p1 = communicator->stringToProxy("test -e 1.0:opaque -e 1.0 -t 1 -v CTEyNy4wLjAuMeouAAAQJwAAAA==:opaque -e 1.0 -t 1 -v CTEyNy4wLjAuMusuAAAQJwAAAA==");
pstr = communicator->proxyToString(p1);
test(pstr == "test -t -e 1.0:tcp -h 127.0.0.1 -p 12010 -t 10000:tcp -h 127.0.0.2 -p 12011 -t 10000");
//
// Test that an SSL endpoint and a nonsense endpoint get written
// back out as an opaque endpoint.
//
p1 = communicator->stringToProxy(
"test -e 1.0:opaque -e 1.0 -t 2 -v CTEyNy4wLjAuMREnAAD/////AA==:opaque -e 1.0 -t 99 -v abch");
pstr = communicator->proxyToString(p1);
if(ssl)
{
test(pstr == "test -t -e 1.0:ssl -h 127.0.0.1 -p 10001 -t infinite:opaque -t 99 -e 1.0 -v abch");
}
else if(tcp)
{
test(pstr ==
"test -t -e 1.0:opaque -t 2 -e 1.0 -v CTEyNy4wLjAuMREnAAD/////AA==:opaque -t 99 -e 1.0 -v abch");
}
//
// Try to invoke on the endpoint to verify that we get a
// NoEndpointException (or ConnectionRefusedException when
// running with SSL).
//
if(ssl)
{
try
{
p1->ice_encodingVersion(Ice::Encoding_1_0)->ice_ping();
test(false);
}
catch(const Ice::ConnectFailedException&)
{
}
}
//
// Test that the proxy with an SSL endpoint and a nonsense
// endpoint (which the server doesn't understand either) can be
// sent over the wire and returned by the server without losing
// the opaque endpoints.
//
Ice::ObjectPrx p2 = derived->echo(p1);
pstr = communicator->proxyToString(p2);
if(ssl)
{
test(pstr == "test -t -e 1.0:ssl -h 127.0.0.1 -p 10001 -t infinite:opaque -t 99 -e 1.0 -v abch");
}
else if(tcp)
{
test(pstr ==
"test -t -e 1.0:opaque -t 2 -e 1.0 -v CTEyNy4wLjAuMREnAAD/////AA==:opaque -t 99 -e 1.0 -v abch");
}
}
cout << "ok" << endl;
return cl;
}
int main(int argc, char** argv)
{
int status;
// ICE
Ice::CommunicatorPtr ic;
jderobot::cameraClient* camRGB = NULL; // parallelIce RGB Image
jderobot::Pose3DPrx pose3Dprx = 0; // ICE Pose3D proxy
IceUtil::ThreadControl rgbTc; // RGB Image Thread Control
pthread_t thr_gui; // GUI thread
pthread_t thr_camera; // Update Camera thread
pthread_t thr_speed; //speed control
std::string prefix("navigatorCamera"); // Component Prefix
std::string gladeFile; // Path to the glade file
bool guiActivated; // GUI activation flag
bool controlActivated; // Control activation flag
bool speedActivated;
int pose3dFps; // Frequency to update the Pose3D.
long cycle_pose3d; // Time cycle for control the update of Pose3D.
sharer = new navigatorCamera::Sharer();
struct timeval a, b;
long totala, totalb;
long diff;
//--------------------INPUT ARGUMENTS--------------------//
if (argc != 2)
{
std::cerr << "\nUSE: ./" << prefix << " --Ice.Config=" << prefix << ".cfg\n" << std::endl;
return 1;
}
//------------------END INPUT ARGUMENTS------------------//
try
{
//--------------------ICE--------------------//
ic = Ice::initialize(argc,argv);
Ice::PropertiesPtr prop = ic->getProperties();
gladeFile = prop->getPropertyWithDefault(prefix + ".gladeFile", "./" + prefix + ".glade");
guiActivated = prop->getPropertyAsIntWithDefault(prefix + ".guiActivated",1);
controlActivated = prop->getPropertyAsIntWithDefault(prefix + ".controlActivated",0);
speedActivated = prop->getPropertyAsIntWithDefault(prefix + ".speedActivated",0);
pose3dFps = prop->getPropertyAsIntWithDefault(prefix + ".Pose3D.Fps",25);
cycle_pose3d = (long)( (1./(float)pose3dFps) * 1000000); //microseconds
sharer->setTranslationStep(atof(prop->getPropertyWithDefault(prefix + ".TranslationStep","0.1").c_str()));
sharer->setRotationStep(atof(prop->getPropertyWithDefault(prefix + ".RotationStep","0.1").c_str()));
// Contact to RGB Image interface
camRGB = new jderobot::cameraClient(ic, prefix + ".CameraRGB.");
if (camRGB != NULL){
rgbTc = camRGB->start();
}
else{
throw prefix + ": failed to load RGB Camera";
}
// Contact to Pose3D interface
Ice::ObjectPrx basePose3D = ic->propertyToProxy(prefix + ".Pose3D.Proxy");
if ( basePose3D == 0 )
throw "Could not create proxy with Pose3D.";
// Cast to Pose3D
pose3Dprx = jderobot::Pose3DPrx::checkedCast(basePose3D);
if ( pose3Dprx == 0 )
throw std::invalid_argument("Invalid proxy " + prefix + ".Pose3D.Proxy");
//------------------END ICE------------------//
sharer->setGuiVisible(guiActivated);
sharer->setControlActive(controlActivated);
if ( guiActivated )
pthread_create(&thr_gui, NULL, &showGui, static_cast<void *>(&gladeFile));
pthread_create(&thr_camera, NULL, &updateCamera, static_cast<void *>(camRGB));
// Captures Pose3D ICE data.
jderobot::Pose3DDataPtr p3dData = pose3Dprx->getPose3DData();
sharer->setPose3D(p3dData);
if ( speedActivated ) {
pthread_create(&thr_speed, NULL, &updateSpeed, NULL);
}
while ( sharer->getControlActive() || sharer->getGuiVisible() )
{
gettimeofday(&a, NULL);
totala = a.tv_sec * 1000000 + a.tv_usec;
// Update the current Pose3D ICE data.
pose3Dprx->setPose3DData(sharer->getPose3D());
// Sleep Algorithm
gettimeofday(&b, NULL);
totalb = b.tv_sec * 1000000 + b.tv_usec;
diff = (totalb - totala);
cycleWait(cycle_pose3d, diff);
}
if ( guiActivated )
pthread_join(thr_gui, NULL);
pthread_join(thr_camera, NULL);
camRGB->stop_thread();
} catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
status = 1;
} catch (const std::exception& ex) {
std::cerr << ex.what() << std::endl;
status = 1;
} catch (const char* msg) {
std::cerr << "Error: " << msg << std::endl;
status = 1;
}
rgbTc.join();
if ( ic )
ic->destroy();
return status;
} //end main
int
run(int argc, char* argv[], const Ice::CommunicatorPtr& communicator)
{
if(argc > 1)
{
fprintf(stderr, "%s: too many arguments", argv[0]);
return EXIT_FAILURE;
}
HelloPrx hello = HelloPrx::checkedCast(communicator->propertyToProxy("Hello.Proxy"));
if(!hello)
{
fprintf(stderr, "%s: invalid proxy\n", argv[0]);
return EXIT_FAILURE;
}
menu();
char c = EOF;
do
{
try
{
printf("==> "); fflush(stdout);
do
{
c = getchar();
}
while(c != EOF && c == '\n');
if(c == 'i')
{
hello->sayHello(0);
}
else if(c == 'd')
{
hello->sayHello_async(new AMI_Hello_sayHelloI, 5000);
}
else if(c == 's')
{
hello->shutdown();
}
else if(c == 'x')
{
// Nothing to do
}
else if(c == '?')
{
menu();
}
else
{
printf("unknown command `%c'\n", c);
menu();
}
}
catch(const Ice::Exception& ex)
{
fprintf(stderr, "%s\n", ex.toString().c_str());
}
}
while(c != EOF && c != 'x');
return EXIT_SUCCESS;
}
int main(int argc, char** argv){
Ice::CommunicatorPtr ic;
Ice::PropertiesPtr prop;
std::string prefix("replayController.");
jderobot::replayControlPrx prx;
try{
ic = Ice::initialize(argc,argv);
prop = ic->getProperties();
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
return 1;
}
catch (const char* msg) {
std::cerr <<"Error :" << msg << std::endl;
return 1;
}
try{
Ice::ObjectPrx base = ic->propertyToProxy(prefix+"control.Proxy");
if (0==base){
throw "replayController: Could not create proxy with replayControl";
}
else {
prx= jderobot::replayControlPrx::checkedCast(base);
if (0==prx)
throw "Invalid " + prefix + ".Proxy";
}
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
}
catch (const char* msg) {
std::cerr << msg << std::endl;
std::cout << "replayController: Not camera provided" << std::endl;
}
int ips=prop->getPropertyAsIntWithDefault(prefix+"IPS",10);
float cycle=(float)(1/(float)ips)*1000000;
replayController::replayControllergui* gui = new replayController::replayControllergui(prx);
struct timeval post;
long long int totalpre=0;
long long int totalpost=0;
while (gui->update()){
gettimeofday(&post,NULL);
totalpost=post.tv_sec*1000000+post.tv_usec;
if (totalpre !=0){
if ((totalpost - totalpre) > cycle ){
std::cout<<"-------- replayController: camera adquisition timeout-" << std::endl;
}
else{
usleep(cycle - (totalpost - totalpre));
}
}
totalpre=totalpost;
}
}
int
run(int argc, char* argv[], const Ice::CommunicatorPtr& communicator1, const Ice::CommunicatorPtr& communicator2)
{
if(argc > 1)
{
cerr << argv[0] << ": too many arguments" << endl;
return EXIT_FAILURE;
}
const string proxyProperty = "Greet.Proxy";
GreetPrx greet1 = GreetPrx::checkedCast(communicator1->propertyToProxy(proxyProperty));
if(!greet1)
{
cerr << argv[0] << ": invalid proxy" << endl;
return EXIT_FAILURE;
}
GreetPrx greet2 = GreetPrx::checkedCast(communicator2->propertyToProxy(proxyProperty));
if(!greet2)
{
cerr << argv[0] << ": invalid proxy" << endl;
return EXIT_FAILURE;
}
menu();
string greeting = "Bonne journ\351e";
char c;
do
{
try
{
cout << "==> ";
cin >> c;
if(c == 't')
{
string ret = greet1->exchangeGreeting(greeting);
cout << "Received: \"" << decodeString(ret) << '\"' << endl;
}
else if(c == 'u')
{
string ret = greet2->exchangeGreeting(greeting);
cout << "Received: \"" << decodeString(ret) << '\"' << endl;
}
else if(c == 's')
{
greet1->shutdown();
}
else if(c == 'x')
{
// Nothing to do
}
else if(c == '?')
{
menu();
}
else
{
cout << "unknown command `" << c << "'" << endl;
menu();
}
}
catch(const Ice::Exception& ex)
{
cerr << ex << endl;
}
}
while(cin.good() && c != 'x');
return EXIT_SUCCESS;
}
int
run(const Ice::StringSeq& args)
{
IceUtilInternal::Options opts;
opts.addOpt("h", "help");
opts.addOpt("v", "version");
vector<string> commands;
try
{
commands = opts.parse(args);
}
catch(const IceUtilInternal::BadOptException& e)
{
consoleErr << e.reason << endl;
usage(args[0]);
return 1;
}
if(opts.isSet("help"))
{
usage(args[0]);
return 0;
}
if(opts.isSet("version"))
{
consoleOut << ICE_STRING_VERSION << endl;
return 0;
}
if(commands.empty())
{
usage(args[0]);
return 1;
}
Ice::ObjectPrxPtr base = communicator->propertyToProxy("IceBoxAdmin.ServiceManager.Proxy");
if(base == 0)
{
//
// The old deprecated way to retrieve the service manager proxy
//
Ice::PropertiesPtr properties = communicator->getProperties();
Ice::Identity managerIdentity;
managerIdentity.category = properties->getPropertyWithDefault("IceBox.InstanceName", "IceBox");
managerIdentity.name = "ServiceManager";
string managerProxy;
if(properties->getProperty("Ice.Default.Locator").empty())
{
string managerEndpoints = properties->getProperty("IceBox.ServiceManager.Endpoints");
if(managerEndpoints.empty())
{
consoleErr << args[0] << ": property `IceBoxAdmin.ServiceManager.Proxy' is not set" << endl;
return 1;
}
managerProxy = "\"" + communicator->identityToString(managerIdentity) + "\" :" + managerEndpoints;
}
else
{
string managerAdapterId = properties->getProperty("IceBox.ServiceManager.AdapterId");
if(managerAdapterId.empty())
{
consoleErr << args[0] << ": property `IceBoxAdmin.ServiceManager.Proxy' is not set" << endl;
return 1;
}
managerProxy = "\"" + communicator->identityToString(managerIdentity) + "\" @" + managerAdapterId;
}
base = communicator->stringToProxy(managerProxy);
}
IceBox::ServiceManagerPrxPtr manager = ICE_CHECKED_CAST(IceBox::ServiceManagerPrx, base);
if(!manager)
{
consoleErr << args[0] << ": `" << base << "' is not an IceBox::ServiceManager" << endl;
return 1;
}
for(vector<string>::const_iterator r = commands.begin(); r != commands.end(); ++r)
{
if((*r) == "shutdown")
{
manager->shutdown();
}
else if((*r) == "start")
{
if(++r == commands.end())
{
consoleErr << args[0] << ": no service name specified." << endl;
return 1;
}
try
{
manager->startService(*r);
}
catch(const IceBox::NoSuchServiceException&)
{
consoleErr << args[0] << ": unknown service `" << *r << "'" << endl;
return 1;
}
catch(const IceBox::AlreadyStartedException&)
{
consoleErr << args[0] << ": service already started." << endl;
}
}
else if((*r) == "stop")
{
if(++r == commands.end())
{
consoleErr << args[0] << ": no service name specified." << endl;
return 1;
}
try
{
manager->stopService(*r);
}
catch(const IceBox::NoSuchServiceException&)
{
consoleErr << args[0] << ": unknown service `" << *r << "'" << endl;
return 1;
}
catch(const IceBox::AlreadyStoppedException&)
{
consoleErr << args[0] << ": service already stopped." << endl;
}
}
else
{
consoleErr << args[0] << ": unknown command `" << *r << "'" << endl;
usage(args[0]);
return 1;
}
}
return 0;
}
Sensors::Sensors(Ice::CommunicatorPtr ic)
{
this-> ic = ic;
Ice::PropertiesPtr prop = ic->getProperties();
////////////////////////////// Pose3D //////////////////////////////
// Contact to POSE3D interface
Ice::ObjectPrx basePose3D = ic->propertyToProxy("kobukiViewer.Pose3D.Proxy");
if (0 == basePose3D) {
pose3dON = false;
std::cout << "Pose3D configuration not specified" <<std::endl;
//throw "Could not create proxy with pose3D";
}else{
// Cast to pose3D
try {
p3dprx = jderobot::Pose3DPrx::checkedCast(basePose3D);
if (0 == p3dprx)
throw "Invalid proxy kobukiViewer.Pose3D.Proxy";
pose3dON = true;
std::cout << "Pose3D connected" << std::endl;
}catch (Ice::ConnectionRefusedException& e){
pose3dON=false;
std::cout << "Pose3D inactive" << std::endl;
}
}
////////////////////////////// CAMERA1 /////////////////////////////
jderobot::ImageDataPtr data;
Ice::ObjectPrx baseCamera1 = ic->propertyToProxy("kobukiViewer.Camera1.Proxy");
if (0==baseCamera1) {
camera1ON = false;
image1.create(400, 400, CV_8UC3);
std::cout << "Camera1 configuration not specified" <<std::endl;
//throw "Could not create proxy";
}else{
/*cast to CameraPrx*/
try {
camera1 = jderobot::CameraPrx::checkedCast(baseCamera1);
if (0==camera1)
throw "Invalid proxy";
camera1ON = true;
std::cout << "Camera1 connected" << std::endl;
data = camera1->getImageData(camera1->getImageFormat().at(0));
image1.create(data->description->height, data->description->width, CV_8UC3);
}catch (Ice::ConnectionRefusedException& e){
camera1ON=false;
std::cout << "Camera1 inactive" << std::endl;
//create an empty image if no camera connected (avoid seg. fault)
image1.create(400, 400, CV_8UC3);
}}
////////////////////////////// CAMERA2 /////////////////////////////
Ice::ObjectPrx baseCamera2 = ic->propertyToProxy("kobukiViewer.Camera2.Proxy");
if (0==baseCamera2) {
camera2ON = false;
image2.create(400, 400, CV_8UC3);
std::cout << "Camera2 configuration not specified" <<std::endl;
//throw "Could not create proxy";
}else{
/*cast to CameraPrx*/
try {
camera2 = jderobot::CameraPrx::checkedCast(baseCamera2);
if (0==camera2)
throw "Invalid proxy";
camera2ON = true;
std::cout << "Camera2 connected" << std::endl;
data = camera2->getImageData(camera2->getImageFormat().at(0));
image2.create(data->description->height, data->description->width, CV_8UC3);
}catch (Ice::ConnectionRefusedException& e){
camera2ON=false;
std::cout << "Camera2 inactive" << std::endl;
//create an empty image if no camera connected (avoid seg. fault)
image2.create(400, 400, CV_8UC3);
}}
////////////////////////////// LASER //////////////////////////////
// Contact to LASER interface
laserICE = ic->propertyToProxy("kobukiViewer.Laser1.Proxy");
if (0 == laserICE) {
laser1ON = false;
std::cout << "Laser configuration not specified" <<std::endl;
//throw "Could not create proxy with Laser";
}else{
// Cast to LASER
try {
laserprx1 = jderobot::LaserPrx::checkedCast(laserICE);
if (0 == laserprx1){
throw std::string("Invalid proxy kobukiViewer.Laser1.Proxy");
}
laser1ON = true;
std::cout << "Laser connected" << std::endl;
}catch (Ice::ConnectionRefusedException& e){
laser1ON=false;
std::cout << "Laser inactive" << std::endl;
}}
// Contact to LASER interface
laserICE = ic->propertyToProxy("kobukiViewer.Laser2.Proxy");
if (0 == laserICE) {
laser2ON = false;
std::cout << "Laser configuration not specified" <<std::endl;
//throw "Could not create proxy with Laser";
}else{
// Cast to LASER
try {
laserprx2 = jderobot::LaserPrx::checkedCast(laserICE);
if (0 == laserprx2){
throw std::string("Invalid proxy kobukiViewer.Laser2.Proxy");
}
laser2ON = true;
std::cout << "Laser connected" << std::endl;
}catch (Ice::ConnectionRefusedException& e){
laser2ON=false;
std::cout << "Laser inactive" << std::endl;
}}
// Contact to LASER interface
laserICE = ic->propertyToProxy("kobukiViewer.Laser3.Proxy");
if (0 == laserICE) {
laser3ON = false;
std::cout << "Laser configuration not specified" <<std::endl;
//throw "Could not create proxy with Laser";
}else{
// Cast to LASER
try {
laserprx3 = jderobot::LaserPrx::checkedCast(laserICE);
if (0 == laserprx3){
throw std::string("Invalid proxy kobukiViewer.Laser3.Proxy");
}
laser3ON = true;
std::cout << "Laser connected" << std::endl;
}catch (Ice::ConnectionRefusedException& e){
laser3ON=false;
std::cout << "Laser inactive" << std::endl;
}}
/*boolLaser = prop->getPropertyAsInt("kobukiViewer.Laser");
std::cout << "Laser " << boolLaser << std::endl;
if(boolLaser){
// Contact to LASER interface
Ice::ObjectPrx laserICE = ic->propertyToProxy("kobukiViewer.Laser.Proxy");
if (0 == laserICE)
throw "Could not create proxy with Laser";
// Cast to LASER
laserprx = jderobot::LaserPrx::checkedCast(laserICE);
if (0 == laserprx){
throw std::string("Invalid proxy kobukiViewer.Laser.Proxy");
}
}*/
}
int main(int argc, char** argv){
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = exitApplication;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
int accion=0;
int sentido=10;
int status;
struct timeval a, b, t2;
int cycle = 100;
long totalb,totala;
long diff;
// INTERFACE
std::vector<jderobot::LaserPrx> lprx;
std::vector<jderobot::CameraPrx> cprx;
std::vector<jderobot::Pose3DEncodersPrx> pose3dencoders;
std::vector<jderobot::Pose3DPrx> pose3dprx;
std::vector<jderobot::EncodersPrx> encoders;
std::vector <jderobot::pointCloudPrx> prx;
//INTERFACE DATA
jderobot::EncodersDataPtr ed;
jderobot::LaserDataPtr ld;
jderobot::ImageDataPtr imageData;
//pools
pthread_attr_t attr;
//images
std::vector<recorder::poolWriteImages*> poolImages;
int nConsumidores;
int poolSize;
//lasers
std::vector<recorder::poolWriteLasers*> poolLasers;
//pose3dencoders
std::vector<recorder::poolWritePose3dEncoders*> poolPose3dEncoders;
//pose3d
std::vector<recorder::poolWritePose3d*> poolPose3d;
//encoders
std::vector<recorder::poolWriteEncoders*> poolEncoders;
//pointClouds
std::vector<recorder::poolWritePointCloud*> poolPointClouds;
//numero de lasers
int Hz = 10;
int muestrasLaser = 180;
int pngCompressRatio;
int jpgQuality;
std::string fileFormat;
std::vector<int> compression_params;
//---------------- INPUT ARGUMENTS ---------------//
if (argc<2){
std::cout << std::endl << "USE: ./mycomponent --Ice.Config=mycomponent.cfg" << std::endl;
}
//---------------- INPUT ARGUMENTS -----------------//
try{
//creamos el directorio principal
struct stat buf;
char dire[]="./data/";
if( stat( dire, &buf ) == -1 )
{
system("mkdir data");
}
Ice::PropertiesPtr prop;
ic = Ice::initialize(argc,argv);
prop = ic->getProperties();
Hz = prop->getPropertyAsInt("Recorder.Hz");
cycle = 1000.0/Hz;
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
nCameras = prop->getPropertyAsIntWithDefault("Recorder.nCameras",0);
if (nCameras > 0 ){
struct stat buf;
char dire[]="./data/images/";
if( stat( dire, &buf ) == -1 )
{
system("mkdir ./data/images/");
}
fileFormat=prop->getProperty("Recorder.FileFormat");
if (fileFormat.compare(std::string("png"))==0){
pngCompressRatio=prop->getPropertyAsIntWithDefault("Recorder.PngCompression",3);
compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
compression_params.push_back(pngCompressRatio);
}
else if (fileFormat.compare(std::string("jpg"))==0){
jpgQuality=prop->getPropertyAsIntWithDefault("Recorder.JpgQuality",95);
compression_params.push_back(CV_IMWRITE_JPEG_QUALITY);
compression_params.push_back(jpgQuality);
}
else{
throw "File format is not valid";
}
nConsumidores=prop->getPropertyAsIntWithDefault("Recorder.nConsumers",2);
poolSize=prop->getPropertyAsIntWithDefault("Recorder.poolSize",10);
}
for (int i=0; i< nCameras; i++){
struct stat buf;
std::stringstream cameraPath;
cameraPath << "./data/images/camera" << i+1;
if( stat( cameraPath.str().c_str(), &buf ) == -1 )
{
std::stringstream instruction;
instruction << "mkdir " << cameraPath.str();
system(instruction.str().c_str());
}
std::stringstream sProxy;
// Get driver camera
sProxy << "Recorder.Camera" << i+1 << ".Proxy";
Ice::ObjectPrx camara = ic->propertyToProxy(sProxy.str());
if (0==camara)
throw "Could not create proxy to camera1 server";
// cast to CameraPrx
jderobot::CameraPrx cprxAux = jderobot::CameraPrx::checkedCast(camara);
if (0== cprxAux)
throw "Invalid proxy";
else
cprx.push_back(cprxAux);
std::stringstream sFormat;
std::string imageFormat;
sFormat << "Recorder.Camera" << i+1 << ".Format";
imageFormat = prop->getProperty(sFormat.str());
//pool
recorder::poolWriteImages *temp = new recorder::poolWriteImages(cprxAux, Hz,poolSize,i+1,imageFormat ,fileFormat ,compression_params);
poolImages.push_back(temp);
for (int j=i*nConsumidores; j< i*nConsumidores+nConsumidores; j++){
pthread_create(&consumerThreads[j], &attr, camera_pool_consumer_thread,temp);
totalConsumers++;
}
pthread_create(&producerThreads[i], &attr, camera_pool_producer_thread,temp);
totalProducers++;
}
nLasers= prop->getPropertyAsInt("Recorder.nLasers");
if (nLasers > 0){
struct stat buf;
char dire[]="./data/lasers/";
if( stat( dire, &buf ) == -1 )
{
system("mkdir data/lasers/");
}
}
for (int i=0; i< nLasers; i++){
struct stat buf;
std::stringstream claserPath;
claserPath << "./data/lasers/laser" << i+1;
if( stat( claserPath.str().c_str(), &buf ) == -1 )
{
std::stringstream instruction;
instruction << "mkdir " << claserPath.str();
system(instruction.str().c_str());
}
// Contact to LASER interface
std::stringstream sProxy;
sProxy << "Recorder.Laser" << i+1 << ".Proxy";
Ice::ObjectPrx baseLaser = ic->propertyToProxy(sProxy.str());
if (0==baseLaser)
throw "Could not create proxy with laser";
// Cast to laser
jderobot::LaserPrx laserPrx = jderobot::LaserPrx::checkedCast(baseLaser);
if (0== laserPrx)
throw "Invalid proxy Mycomponent.Laser.Proxy";
lprx.push_back(laserPrx);
recorder::poolWriteLasers *temp = new recorder::poolWriteLasers(laserPrx, Hz,poolSize,i+1);
poolLasers.push_back(temp);
pthread_create(&consumerThreads[totalConsumers], &attr, laser_pool_consumer_thread,temp);
totalConsumers++;
pthread_create(&producerThreads[totalProducers], &attr, laser_pool_producer_thread,temp);
totalProducers++;
}
nPose3dEncoders= prop->getPropertyAsInt("Recorder.nPose3dEncoders");
if (nPose3dEncoders > 0){
struct stat buf;
char dire[]="./data/pose3dencoders/";
if( stat( dire, &buf ) == -1 )
{
system("mkdir data/pose3dencoders/");
}
}
for (int i=0; i< nPose3dEncoders; i++){
struct stat buf;
std::stringstream claserPath;
claserPath << "./data/pose3dencoders/pose3dencoder" << i+1;
if( stat( claserPath.str().c_str(), &buf ) == -1 )
{
std::stringstream instruction;
instruction << "mkdir " << claserPath.str();
system(instruction.str().c_str());
}
// Contact to POSE3DENCODERS interface
std::stringstream sProxy;
sProxy << "Recorder.Pose3DEncoders" << i+1 << ".Proxy";
Ice::ObjectPrx base = ic->propertyToProxy(sProxy.str());
if (0==base)
throw "Could not create proxy with pose3dencoders";
// Cast to Pose3DEncodersPrx
jderobot::Pose3DEncodersPrx prx = jderobot::Pose3DEncodersPrx::checkedCast(base);
if (0== prx)
throw "Invalid proxy Mycomponent.pose3dencoders.Proxy";
pose3dencoders.push_back(prx);
recorder::poolWritePose3dEncoders*temp = new recorder::poolWritePose3dEncoders(prx, Hz,poolSize,i+1);
poolPose3dEncoders.push_back(temp);
pthread_create(&consumerThreads[totalConsumers], &attr, pose3dencoders_pool_consumer_thread,temp);
totalConsumers++;
pthread_create(&producerThreads[totalProducers], &attr, pose3dencoders_pool_producer_thread,temp);
totalProducers++;
}
nPose3d= prop->getPropertyAsInt("Recorder.nPose3d");
if (nPose3d > 0){
struct stat buf;
char dire[]="./data/pose3d/";
if( stat( dire, &buf ) == -1 )
{
system("mkdir data/pose3d/");
}
}
for (int i=0; i< nPose3d; i++){
struct stat buf;
std::stringstream claserPath;
claserPath << "./data/pose3d/pose3d" << i+1;
if( stat( claserPath.str().c_str(), &buf ) == -1 )
{
std::stringstream instruction;
instruction << "mkdir " << claserPath.str();
system(instruction.str().c_str());
}
// Contact to POSE3D interface
std::stringstream sProxy;
sProxy << "Recorder.Pose3D" << i+1 << ".Proxy";
Ice::ObjectPrx base = ic->propertyToProxy(sProxy.str());
if (0==base)
throw "Could not create proxy with pose3d";
// Cast to Pose3DPrx
jderobot::Pose3DPrx prx = jderobot::Pose3DPrx::checkedCast(base);
if (0== prx)
throw "Invalid proxy Mycomponent.pose3d.Proxy";
pose3dprx.push_back(prx);
recorder::poolWritePose3d*temp = new recorder::poolWritePose3d(prx, Hz,poolSize,i+1);
poolPose3d.push_back(temp);
pthread_create(&consumerThreads[totalConsumers], &attr, pose3d_pool_consumer_thread,temp);
totalConsumers++;
pthread_create(&producerThreads[totalProducers], &attr, pose3d_pool_producer_thread,temp);
totalProducers++;
}
nEncoders= prop->getPropertyAsInt("Recorder.nEncoders");
if (nEncoders > 0){
struct stat buf;
char dire[]="./data/encoders/";
if( stat( dire, &buf ) == -1 )
{
system("mkdir data/encoders/");
}
}
for (int i=0; i< nEncoders; i++){
struct stat buf;
std::stringstream claserPath;
claserPath << "./data/encoders/encoder" << i+1;
if( stat( claserPath.str().c_str(), &buf ) == -1 )
{
std::stringstream instruction;
instruction << "mkdir " << claserPath.str();
system(instruction.str().c_str());
}
// Contact to ENCODERS interface
std::stringstream sProxy;
sProxy << "Recorder.Encoders" << i+1 << ".Proxy";
Ice::ObjectPrx base = ic->propertyToProxy(sProxy.str());
if (0==base)
throw "Could not create proxy with encoders";
// Cast to EncodersPrx
jderobot::EncodersPrx prx = jderobot::EncodersPrx::checkedCast(base);
if (0== prx)
throw "Invalid proxy Mycomponent.encoders.Proxy";
encoders.push_back(prx);
recorder::poolWriteEncoders*temp = new recorder::poolWriteEncoders(prx, Hz,poolSize,i+1);
poolEncoders.push_back(temp);
pthread_create(&consumerThreads[totalConsumers], &attr, encoders_pool_consumer_thread,temp);
totalConsumers++;
pthread_create(&producerThreads[totalProducers], &attr, encoders_pool_producer_thread,temp);
totalProducers++;
}
nDepthSensors = prop->getPropertyAsIntWithDefault("Recorder.nDethSensors",0);
if (nDepthSensors){
struct stat buf;
char dire[]="./data/pointClouds/";
if( stat( dire, &buf ) == -1 )
{
system("mkdir data/pointClouds/");
}
}
for (int i=0; i< nDepthSensors; i++){
struct stat buf;
std::stringstream claserPath;
claserPath << "./data/pointClouds/pointCloud" << i+1;
if( stat( claserPath.str().c_str(), &buf ) == -1 )
{
std::stringstream instruction;
instruction << "mkdir " << claserPath.str();
system(instruction.str().c_str());
}
std::stringstream sProxy;
// Get driver camera
sProxy << "Recorder.DepthSensor" << i+1 << ".Proxy";
Ice::ObjectPrx kinect = ic->propertyToProxy(sProxy.str());
if (0==kinect){
throw "Could not create proxy with Kinect1";
}
// Cast to KINECT
jderobot::pointCloudPrx prxAux = jderobot::pointCloudPrx::checkedCast(kinect);
if (0== prxAux){
throw std::string("Invalid proxy Recorder.Kinect1.Proxy");
}
prx.push_back(prxAux);
recorder::poolWritePointCloud* temp = new recorder::poolWritePointCloud(prxAux, Hz,poolSize,i+1);
poolPointClouds.push_back(temp);
pthread_create(&consumerThreads[totalConsumers], &attr, pointcloud_pool_consumer_thread,temp);
totalConsumers++;
pthread_create(&producerThreads[totalProducers], &attr, pointcloud_pool_producer_thread,temp);
totalProducers++;
}
//****************************** Processing the Control ******************************///
//---------------- ITERATIONS CONTROL -----------//
//muestreo para el laser
muestrasLaser = prop->getPropertyAsInt("Recorder.Laser.Samples");
std::string robotName = prop->getPropertyWithDefault("Recorder.Hostname","localhost");
std::string robotPort = prop->getPropertyWithDefault("Recorder.Port","9999");
long timeRelative = 0;
int guiActive=prop->getPropertyAsIntWithDefault("Recorder.GUI",0);
recorder::recordergui *gui;
if (guiActive){
gui = new recorder::recordergui();
}
long long int iteration=0;
while(globalActive){
//gui activado
if (guiActive){
gui->set_iteration(iteration);
gui->update();
globalActive=gui->get_active();
recording=gui->get_recording();
}
else{
recording=true;
}
if (recording){
if (iteration==0){
gettimeofday(&inicio,NULL);
}
iteration++;
gettimeofday(&b,NULL);
totalb=b.tv_sec*1000000+b.tv_usec;
//calculamos la velocidad de grabación actual
float myfps=1000000./((float)totalb-(float)totala);
if (guiActive){
gui->set_fps((int)myfps);
}
gettimeofday(&a,NULL);
totala=a.tv_sec*1000000+a.tv_usec;
gettimeofday(&b,NULL);
totalb=b.tv_sec*1000000+b.tv_usec;
std::cout << "Recorder takes " << (totalb-totala)/1000 << " ms" << std::endl;
diff = (totalb-totala)/1000;
if(diff < 0 || diff > cycle)
diff = cycle;
else
diff = cycle-diff;
//Sleep Algorithm
usleep(diff*1000);
if(diff < 10)
usleep(10*1000);
// std::cout << cycle <<" ->" << diff << " ->" << timeRelative<< std::endl;
timeRelative+= diff + (totalb-totala)/1000;
// std::cout << "->" << diff << " ->" << timeRelative<< std::endl;
}
else{
usleep(10*1000);
}
}
//--------------ITERATIONS CONTROL-------------//
}
catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
status = 1;
}
catch (const char* msg) {
std::cerr << msg << std::endl;
status = 1;
}
if (!killed)
exitApplication(1);
return 0;
}
int main(int argc, char** argv){
int status;
giraffeClient::View * view;
giraffeClient::Controller * controller;
Ice::CommunicatorPtr ic;
RoboCompJointMotor::MotorParamsList motorsparams;
RoboCompJointMotor::MotorStateMap motorsstate;
std::cout << "init " << std::endl;
try{
ic = Ice::initialize(argc,argv);
/*Get to giraffe server*/
Ice::ObjectPrx base1 = ic->propertyToProxy("giraffeClient.JointMotor.Proxy");
if (0==base1)
throw "Could not create proxy to giraffe server";
/*cast to JointMotorPrx*/
RoboCompJointMotor::JointMotorPrx jprx = RoboCompJointMotor::JointMotorPrx::checkedCast(base1);
if (0==jprx)
throw "Invalid proxy";
/*Get driver camera*/
Ice::ObjectPrx base2 = ic->propertyToProxy("giraffeClient.Camera.Proxy");
if (0==base2)
throw "Could not create proxy to camera server";
/*cast to CameraPrx*/
jderobot::CameraPrx cprx = jderobot::CameraPrx::checkedCast(base2);
if (0==cprx)
throw "Invalid proxy";
/*Create Controller and View*/
//std::cout << "Antes del view:" << std::endl;
controller = new giraffeClient::Controller(jprx);
view = new giraffeClient::View(controller);
/*Show params of motors*/
/*
motorsparams = jprx->getAllMotorParams();
std::cout << "Motors params:" << std::endl;
for(vector<RoboCompJointMotor::MotorParams>::iterator it = motorsparams.begin(); it != motorsparams.end(); it++) {
cout << endl;
cout << "Name: " << (*it).name << endl;
cout << "Id: " << (int) (*it).busId << endl;
cout << "minPos: " << (*it).minPos << endl;
cout << "maxPos: " << (*it).maxPos << endl;
cout << "maxVel: " << (*it).maxVelocity << endl;
cout << "zeroPos: " << (*it).zeroPos << endl;
cout << "inverted: " << (*it).invertedSign << endl;
}
//Get current pos of motors
jprx->getAllMotorState(motorsstate);
view->setInitialValues(motorsstate["neck"].pos, motorsstate["tilt"].pos, motorsstate["leftPan"].pos, motorsstate["rightPan"].pos);
view->setRealValues(motorsstate["neck"].pos, motorsstate["tilt"].pos, motorsstate["leftPan"].pos, motorsstate["rightPan"].pos);
*/
while(view->isVisible()){
/*Get status of motors*/
//jprx->getAllMotorState(motorsstate);
//view->setRealValues(motorsstate["neck"].pos, motorsstate["tilt"].pos, motorsstate["leftPan"].pos, motorsstate["rightPan"].pos);
/*Get image*/
jderobot::ImageDataPtr data = cprx->getImageData();
colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(data->description->format);
if (!fmt)
throw "Format not supported";
colorspaces::Image image(data->description->width,
data->description->height,
fmt,
&(data->pixelData[0]));
view->display(image);
usleep(10*1000);
}
}catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
status = 1;
} catch (const char* msg) {
std::cerr << msg << std::endl;
status = 1;
}
if (ic)
ic->destroy();
return status;
}
int main (int argc, char** argv) {
initiated = false;
dynamic = false;
lmindex = 0;
countUD = 0;
countDU = 0;
count = 0;
nframes = 0;
avg_vel = 0;
line_pos = FR_H - MARGIN;
#ifdef _OPENCV3
pMOG = cv::createBackgroundSubtractorMOG2();
#else
pMOG = new cv::BackgroundSubtractorMOG;
#endif
heat_map = cv::Mat::zeros(FR_W, FR_H, CV_8UC3);
heat_mapfg = cv::Mat::zeros(FR_W, FR_H, CV_8UC3);
cv::rectangle(heat_map, cv::Point(FR_W/2-40, 0), cv::Point(FR_W/2+40, FR_H), cv::Scalar(50, 50, 50), -1);
cv::line(heat_map, cv::Point(FR_W/2, 0), cv::Point(FR_W/2, FR_H), cv::Scalar(100, 100, 100), 1);
Ice::CommunicatorPtr ic;
jderobot::ArDroneExtraPrx arextraprx;
jderobot::Pose3DPrx poseprx;
jderobot::CMDVelPrx cmdprx;
jderobot::CameraPrx camprx;
jderobot::Pose3DDataPtr pose;
jderobot::ImageDataPtr img;
jderobot::CMDVelDataPtr vel = new jderobot::CMDVelData();
//prespecify checkpoints
landmarks.push_back(cv::Point(40.0, 0.0));
landmarks.push_back(cv::Point(10.0, 0.0));
landmarks.push_back(cv::Point(-20.0, 0.0));
count_arr = new int[landmarks.size()]();
avg_vel_arr = new float[landmarks.size()]();
cv::namedWindow("BLOBS", cv::WINDOW_AUTOSIZE);
count_active = 0;
avg_vel = 0;
try {
ic = Ice::initialize(argc, argv);
Ice::PropertiesPtr prop = ic->getProperties();
Ice::ObjectPrx baseextra = ic->propertyToProxy("BSCounter.Extra.Proxy");
if (0==baseextra)
throw "Could not create proxy";
arextraprx = jderobot::ArDroneExtraPrx::checkedCast(baseextra);
if (0==arextraprx)
throw "ArDroneExtra -> Invalid proxy";
Ice::ObjectPrx basepose = ic->propertyToProxy("BSCounter.Pose3D.Proxy");
if (0==basepose)
throw "Could not create proxy";
poseprx = jderobot::Pose3DPrx::checkedCast(basepose);
if (0==poseprx)
throw "Pose3D -> Invalid proxy";
Ice::ObjectPrx basecmd = ic->propertyToProxy("BSCounter.CMDVel.Proxy");
if (0==basecmd)
throw "Could not create proxy";
cmdprx = jderobot::CMDVelPrx::checkedCast(basecmd);
if (0==cmdprx)
throw "CMDVel -> Invalid proxy";
Ice::ObjectPrx basecam = ic->propertyToProxy("BSCounter.Camera.Proxy");
if (0==basecam)
throw "Could not create proxy";
camprx = jderobot::CameraPrx::checkedCast(basecam);
if (0==camprx)
throw "Camera -> Invalid proxy";
img = camprx->getImageData("RGB8");
} catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
exit(-1);
} catch (const char* msg) {
std::cerr << msg << std::endl;
exit(-1);
}
std::cout << "taking off..\n";
arextraprx->takeoff();
std::cout << "reaching desired height..\n";
vel->linearZ = DRONE_VEL;
vel->linearX = 0;
vel->linearY = 0;
cmdprx->setCMDVelData(vel);
while (1) {
pose = poseprx->getPose3DData();
img = camprx->getImageData("RGB8");
image.create(img->description->height, img->description->width, CV_8UC3);
memcpy((unsigned char*) image.data, &(img->pixelData[0]), image.cols*image.rows*3);
cv::imshow("BLOBS", image);
cv::imshow("HEATMAP", heat_map + heat_mapfg);
cv::waitKey(33);
if (!initiated && abs(pose->z - DRONE_HEIGHT) < EPS) {
vel->linearZ = 0;
cmdprx->setCMDVelData(vel);
initiated = true;
std::cout<< "reached height: "<<pose->z<<" m.\n";
std::cout<< "[STATUS] initialized: moving to 1st checkpoint..\n";
}
if (initiated && !dynamic) {
// set velocity for next checkpoint
vel->linearX = (landmarks[lmindex].x-pose->x)/sqrt((landmarks[lmindex].x-pose->x)*(landmarks[lmindex].x-pose->x)+(landmarks[lmindex].y-pose->y)*(landmarks[lmindex].y-pose->y)+(DRONE_HEIGHT-pose->z)*(DRONE_HEIGHT-pose->z));
vel->linearY = (landmarks[lmindex].y-pose->y)/sqrt((landmarks[lmindex].x-pose->x)*(landmarks[lmindex].x-pose->x)+(landmarks[lmindex].y-pose->y)*(landmarks[lmindex].y-pose->y)+(DRONE_HEIGHT-pose->z)*(DRONE_HEIGHT-pose->z));
vel->linearZ = (DRONE_HEIGHT-pose->z)/sqrt((landmarks[lmindex].x-pose->x)*(landmarks[lmindex].x-pose->x)+(landmarks[lmindex].y-pose->y)*(landmarks[lmindex].y-pose->y)+(DRONE_HEIGHT-pose->z)*(DRONE_HEIGHT-pose->z));
float yaw = (float) atan2(2.0*(pose->q0*pose->q3 + pose->q1*pose->q2), 1 - 2.0*(pose->q2*pose->q2 + pose->q3*pose->q3));
float tempX = cos(yaw)*(vel->linearX) + sin(yaw)*(vel->linearY);
float tempY = -sin(yaw)*(vel->linearX) + cos(yaw)*(vel->linearY);
vel->linearX = tempX;
vel->linearY = tempY;
cmdprx->setCMDVelData(vel);
dynamic = true;
// Update heatmap
std::cout << "[HEATMAP] Current Heatmap: \n";
for (int i=0; i<landmarks.size(); i++)
std::cout << "Cars counted at checkpoint ["<< i+1 <<"]: "<<count_arr[i]<<"\n";
heat_mapfg = cv::Mat::zeros(FR_H, FR_W, CV_8UC3);
for (int i=0; i<landmarks.size(); i++) {
cv::circle(heat_mapfg, cv::Point((landmarks[i].y + 50)*2.4, (landmarks[i].x + 50)*2.4), count_arr[i]*3, cv::Scalar(0, 16*avg_vel_arr[i], 255 - 16*avg_vel_arr[i]), -1);
}
cv::GaussianBlur(heat_mapfg, heat_mapfg, cv::Size(15, 15), 3);
std::cout << "Average speed of cars: "<<avg_vel<<"\n";
std::cout<<"\n";
avg_vel = 0;
count_active = 0;
}
if (dynamic && abs(landmarks[lmindex].x-pose->x)<EPS && abs(landmarks[lmindex].y-pose->y)<EPS) {
std::cout << "Reached checkpoint ["<<lmindex+1<<"]: X="<<pose->x<<"m, Y="<<pose->y<<"m, Z="<<pose->z<<"m\n";
std::cout << "[STATUS] Processing started..\n";
vel->linearX = 0;
vel->linearY = 0;
vel->linearZ = 0;
cmdprx->setCMDVelData(vel);
// process Image for N Frames
while (nframes<N_FRAMES) {
nframes++;
img = camprx->getImageData("RGB8");
image.create(img->description->height, img->description->width, CV_8UC3);
memcpy((unsigned char*) image.data, &(img->pixelData[0]), image.cols*image.rows*3);
processImage(image);
}
count_arr[lmindex]=count;
if (count_active) avg_vel/=count_active;
avg_vel_arr[lmindex] = avg_vel;
++lmindex%=landmarks.size();
nframes = 0;
count = 0;
countDU = 0;
countUD = 0;
std::cout << "[STATUS] Processed "<<N_FRAMES<<" frames. Moving to next checkpoint.\n";
dynamic = false;
}
}
return 0;
}
Test::MyClassPrx
allTests(const Ice::CommunicatorPtr& communicator)
{
tprintf("testing stringToProxy... ");
string ref = "test:default -p 12010 -t 10000";
Ice::ObjectPrx base = communicator->stringToProxy(ref);
test(base);
Ice::ObjectPrx b1 = communicator->stringToProxy("test:tcp");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getAdapterId().empty() && b1->ice_getFacet().empty());
b1 = communicator->stringToProxy("test :tcp");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy(" test :tcp");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy(" test:tcp");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy("'test -f facet':tcp");
test(b1->ice_getIdentity().name == "test -f facet" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
try
{
b1 = communicator->stringToProxy("\"test -f facet':tcp");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("\"test -f facet\"");
test(b1->ice_getIdentity().name == "test -f facet" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy("\"test -f facet@test\"");
test(b1->ice_getIdentity().name == "test -f facet@test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
b1 = communicator->stringToProxy("\"test -f facet@test @test\"");
test(b1->ice_getIdentity().name == "test -f facet@test @test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet().empty());
try
{
b1 = communicator->stringToProxy("test test:tcp");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("test\\040test:tcp");
test(b1->ice_getIdentity().name == "test test" && b1->ice_getIdentity().category.empty());
try
{
b1 = communicator->stringToProxy("test\\777");
test(false);
}
catch(const Ice::IdentityParseException&)
{
}
b1 = communicator->stringToProxy("test\\40test");
test(b1->ice_getIdentity().name == "test test");
// Test some octal and hex corner cases.
b1 = communicator->stringToProxy("test\\4test");
test(b1->ice_getIdentity().name == "test\4test");
b1 = communicator->stringToProxy("test\\04test");
test(b1->ice_getIdentity().name == "test\4test");
b1 = communicator->stringToProxy("test\\004test");
test(b1->ice_getIdentity().name == "test\4test");
b1 = communicator->stringToProxy("test\\1114test");
test(b1->ice_getIdentity().name == "test\1114test");
b1 = communicator->stringToProxy("test\\b\\f\\n\\r\\t\\'\\\"\\\\test");
test(b1->ice_getIdentity().name == "test\b\f\n\r\t\'\"\\test" && b1->ice_getIdentity().category.empty());
b1 = communicator->stringToProxy("category/test");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category");
b1 = communicator->stringToProxy("test@adapter");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getAdapterId() == "adapter");
try
{
b1 = communicator->stringToProxy("id@adapter test");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("category/test@adapter");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId() == "adapter");
b1 = communicator->stringToProxy("category/test@adapter:tcp");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId() == "adapter:tcp");
b1 = communicator->stringToProxy("'category 1/test'@adapter");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category 1" &&
b1->ice_getAdapterId() == "adapter");
b1 = communicator->stringToProxy("'category/test 1'@adapter");
test(b1->ice_getIdentity().name == "test 1" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId() == "adapter");
b1 = communicator->stringToProxy("'category/test'@'adapter 1'");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category" &&
b1->ice_getAdapterId() == "adapter 1");
b1 = communicator->stringToProxy("\"category \\/test@foo/test\"@adapter");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category /test@foo" &&
b1->ice_getAdapterId() == "adapter");
b1 = communicator->stringToProxy("\"category \\/test@foo/test\"@\"adapter:tcp\"");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category == "category /test@foo" &&
b1->ice_getAdapterId() == "adapter:tcp");
b1 = communicator->stringToProxy("id -f facet:tcp");
test(b1->ice_getIdentity().name == "id" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet");
b1 = communicator->stringToProxy("id -f 'facet x':tcp");
test(b1->ice_getIdentity().name == "id" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet x");
b1 = communicator->stringToProxy("id -f \"facet x\":tcp");
test(b1->ice_getIdentity().name == "id" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet x");
try
{
b1 = communicator->stringToProxy("id -f \"facet x");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
try
{
b1 = communicator->stringToProxy("id -f \'facet x");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("test -f facet:tcp");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet" && b1->ice_getAdapterId().empty());
b1 = communicator->stringToProxy("test -f \"facet:tcp\"");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet:tcp" && b1->ice_getAdapterId().empty());
b1 = communicator->stringToProxy("test -f facet@test");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet" && b1->ice_getAdapterId() == "test");
b1 = communicator->stringToProxy("test -f 'facet@test'");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet@test" && b1->ice_getAdapterId().empty());
b1 = communicator->stringToProxy("test -f 'facet@test'@test");
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getFacet() == "facet@test" && b1->ice_getAdapterId() == "test");
try
{
b1 = communicator->stringToProxy("test -f facet@test @test");
test(false);
}
catch(const Ice::ProxyParseException&)
{
}
b1 = communicator->stringToProxy("test");
test(b1->ice_isTwoway());
b1 = communicator->stringToProxy("test -t");
test(b1->ice_isTwoway());
b1 = communicator->stringToProxy("test -o");
test(b1->ice_isOneway());
b1 = communicator->stringToProxy("test -O");
test(b1->ice_isBatchOneway());
b1 = communicator->stringToProxy("test -d");
test(b1->ice_isDatagram());
b1 = communicator->stringToProxy("test -D");
test(b1->ice_isBatchDatagram());
b1 = communicator->stringToProxy("test");
test(!b1->ice_isSecure());
b1 = communicator->stringToProxy("test -s");
test(b1->ice_isSecure());
try
{
b1 = communicator->stringToProxy("test:tcp@adapterId");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
// This is an unknown endpoint warning, not a parse exception.
//
//try
//{
// b1 = communicator->stringToProxy("test -f the:facet:tcp");
// test(false);
//}
//catch(const Ice::EndpointParseException&)
//{
//}
try
{
b1 = communicator->stringToProxy("test::tcp");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
tprintf("ok\n");
tprintf("testing propertyToProxy... ");
Ice::PropertiesPtr prop = communicator->getProperties();
string propertyPrefix = "Foo.Proxy";
prop->setProperty(propertyPrefix, "test:default -p 12010 -t 10000");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getIdentity().name == "test" && b1->ice_getIdentity().category.empty() &&
b1->ice_getAdapterId().empty() && b1->ice_getFacet().empty());
string property;
#ifdef ICEE_HAS_LOCATOR
property = propertyPrefix + ".Locator";
test(!b1->ice_getLocator());
prop->setProperty(property, "locator:default -p 10000");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getLocator() && b1->ice_getLocator()->ice_getIdentity().name == "locator");
prop->setProperty(property, "");
#endif
// Now retest with an indirect proxy.
#ifdef ICEE_HAS_LOCATOR
prop->setProperty(propertyPrefix, "test");
property = propertyPrefix + ".Locator";
prop->setProperty(property, "locator:default -p 10000");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getLocator() && b1->ice_getLocator()->ice_getIdentity().name == "locator");
prop->setProperty(property, "");
#endif
prop->setProperty(propertyPrefix, "test:default -p 12010 -t 10000");
#ifdef ICEE_HAS_ROUTER
property = propertyPrefix + ".Router";
test(!b1->ice_getRouter());
prop->setProperty(property, "router:default -p 10000");
b1 = communicator->propertyToProxy(propertyPrefix);
test(b1->ice_getRouter() && b1->ice_getRouter()->ice_getIdentity().name == "router");
prop->setProperty(property, "");
#endif
tprintf("ok\n");
tprintf("testing ice_getCommunicator... ");
test(base->ice_getCommunicator() == communicator);
tprintf("ok\n");
tprintf("testing proxy methods... ");
test(communicator->identityToString(base->ice_identity(communicator->stringToIdentity("other"))->ice_getIdentity())
== "other");
test(base->ice_facet("facet")->ice_getFacet() == "facet");
test(base->ice_adapterId("id")->ice_getAdapterId() == "id");
test(base->ice_twoway()->ice_isTwoway());
test(base->ice_oneway()->ice_isOneway());
test(base->ice_batchOneway()->ice_isBatchOneway());
test(base->ice_datagram()->ice_isDatagram());
test(base->ice_batchDatagram()->ice_isBatchDatagram());
test(base->ice_secure(true)->ice_isSecure());
test(!base->ice_secure(false)->ice_isSecure());
tprintf("ok\n");
tprintf("testing proxy comparison... ");
test(communicator->stringToProxy("foo") == communicator->stringToProxy("foo"));
test(communicator->stringToProxy("foo") != communicator->stringToProxy("foo2"));
test(communicator->stringToProxy("foo") < communicator->stringToProxy("foo2"));
test(!(communicator->stringToProxy("foo2") < communicator->stringToProxy("foo")));
Ice::ObjectPrx compObj = communicator->stringToProxy("foo");
test(compObj->ice_facet("facet") == compObj->ice_facet("facet"));
test(compObj->ice_facet("facet") != compObj->ice_facet("facet1"));
test(compObj->ice_facet("facet") < compObj->ice_facet("facet1"));
test(!(compObj->ice_facet("facet") < compObj->ice_facet("facet")));
test(compObj->ice_oneway() == compObj->ice_oneway());
test(compObj->ice_oneway() != compObj->ice_twoway());
test(compObj->ice_twoway() < compObj->ice_oneway());
test(!(compObj->ice_oneway() < compObj->ice_twoway()));
test(compObj->ice_secure(true) == compObj->ice_secure(true));
test(compObj->ice_secure(false) != compObj->ice_secure(true));
test(compObj->ice_secure(false) < compObj->ice_secure(true));
test(!(compObj->ice_secure(true) < compObj->ice_secure(false)));
test(compObj->ice_timeout(20) == compObj->ice_timeout(20));
test(compObj->ice_timeout(10) != compObj->ice_timeout(20));
test(compObj->ice_timeout(10) < compObj->ice_timeout(20));
test(!(compObj->ice_timeout(20) < compObj->ice_timeout(10)));
#ifdef ICEE_HAS_LOCATOR
Ice::LocatorPrx loc1 = Ice::LocatorPrx::uncheckedCast(communicator->stringToProxy("loc1:default -p 10000"));
Ice::LocatorPrx loc2 = Ice::LocatorPrx::uncheckedCast(communicator->stringToProxy("loc2:default -p 10000"));
test(compObj->ice_locator(0) == compObj->ice_locator(0));
test(compObj->ice_locator(loc1) == compObj->ice_locator(loc1));
test(compObj->ice_locator(loc1) != compObj->ice_locator(0));
test(compObj->ice_locator(0) != compObj->ice_locator(loc2));
test(compObj->ice_locator(loc1) != compObj->ice_locator(loc2));
test(compObj->ice_locator(0) < compObj->ice_locator(loc1));
test(!(compObj->ice_locator(loc1) < compObj->ice_locator(0)));
test(compObj->ice_locator(loc1) < compObj->ice_locator(loc2));
test(!(compObj->ice_locator(loc2) < compObj->ice_locator(loc1)));
#endif
#ifdef ICEE_HAS_ROUTER
Ice::RouterPrx rtr1 = Ice::RouterPrx::uncheckedCast(communicator->stringToProxy("rtr1:default -p 10000"));
Ice::RouterPrx rtr2 = Ice::RouterPrx::uncheckedCast(communicator->stringToProxy("rtr2:default -p 10000"));
test(compObj->ice_router(0) == compObj->ice_router(0));
test(compObj->ice_router(rtr1) == compObj->ice_router(rtr1));
test(compObj->ice_router(rtr1) != compObj->ice_router(0));
test(compObj->ice_router(0) != compObj->ice_router(rtr2));
test(compObj->ice_router(rtr1) != compObj->ice_router(rtr2));
test(compObj->ice_router(0) < compObj->ice_router(rtr1));
test(!(compObj->ice_router(rtr1) < compObj->ice_router(0)));
test(compObj->ice_router(rtr1) < compObj->ice_router(rtr2));
test(!(compObj->ice_router(rtr2) < compObj->ice_router(rtr1)));
#endif
Ice::Context ctx1;
ctx1["ctx1"] = "v1";
Ice::Context ctx2;
ctx2["ctx2"] = "v2";
test(compObj->ice_context(Ice::Context()) == compObj->ice_context(Ice::Context()));
test(compObj->ice_context(ctx1) == compObj->ice_context(ctx1));
test(compObj->ice_context(ctx1) != compObj->ice_context(Ice::Context()));
test(compObj->ice_context(Ice::Context()) != compObj->ice_context(ctx2));
test(compObj->ice_context(ctx1) != compObj->ice_context(ctx2));
test(compObj->ice_context(ctx1) < compObj->ice_context(ctx2));
test(!(compObj->ice_context(ctx2) < compObj->ice_context(ctx1)));
Ice::ObjectPrx compObj1 = communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 10000");
Ice::ObjectPrx compObj2 = communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 10001");
test(compObj1 != compObj2);
test(compObj1 < compObj2);
test(!(compObj2 < compObj1));
compObj1 = communicator->stringToProxy("foo@MyAdapter1");
compObj2 = communicator->stringToProxy("foo@MyAdapter2");
test(compObj1 != compObj2);
test(compObj1 < compObj2);
test(!(compObj2 < compObj1));
#ifdef ICEE_HAS_LOCATOR
compObj1 = communicator->stringToProxy("foo:tcp -h 127.0.0.1 -p 1000");
compObj2 = communicator->stringToProxy("foo@MyAdapter1");
test(compObj1 != compObj2);
test(compObj1 < compObj2);
test(!(compObj2 < compObj1));
#endif
//
// TODO: Ideally we should also test comparison of fixed proxies.
//
tprintf("ok\n");
tprintf("testing checked cast... ");
Test::MyClassPrx cl = Test::MyClassPrx::checkedCast(base);
test(cl);
Test::MyDerivedClassPrx derived = Test::MyDerivedClassPrx::checkedCast(cl);
test(derived);
test(cl == base);
test(derived == base);
test(cl == derived);
#ifdef ICEE_HAS_LOCATOR
Ice::LocatorPrx loc = Ice::LocatorPrx::checkedCast(base);
test(loc == 0);
#endif
//
// Upcasting
//
Test::MyClassPrx cl2 = Test::MyClassPrx::checkedCast(derived);
Ice::ObjectPrx obj = Ice::ObjectPrx::checkedCast(derived);
test(cl2);
test(obj);
test(cl2 == obj);
test(cl2 == derived);
//
// Now with alternate API
//
cl = checkedCast<Test::MyClassPrx>(base);
test(cl);
derived = checkedCast<Test::MyDerivedClassPrx>(cl);
test(derived);
test(cl == base);
test(derived == base);
test(cl == derived);
#ifdef ICEE_HAS_LOCATOR
loc = checkedCast<Ice::LocatorPrx>(base);
test(loc == 0);
#endif
cl2 = checkedCast<Test::MyClassPrx>(derived);
obj = checkedCast<Ice::ObjectPrx>(derived);
test(cl2);
test(obj);
test(cl2 == obj);
test(cl2 == derived);
tprintf("ok\n");
tprintf("testing checked cast with context... ");
Ice::Context c = cl->getContext();
test(c.size() == 0);
c["one"] = "hello";
c["two"] = "world";
cl = Test::MyClassPrx::checkedCast(base, c);
Ice::Context c2 = cl->getContext();
test(c == c2);
//
// Now with alternate API
//
cl = checkedCast<Test::MyClassPrx>(base);
c = cl->getContext();
test(c.size() == 0);
cl = checkedCast<Test::MyClassPrx>(base, c);
c2 = cl->getContext();
test(c == c2);
tprintf("ok\n");
#ifdef ICEE_HAS_OPAQUE_ENDPOINTS
tprintf("testing opaque endpoints... ");
try
{
// Invalid -x option
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 99 -v abc -x abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing -t and -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Repeated -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 1 -t 1 -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Repeated -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 1 -v abc -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 1");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing arg for -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Missing arg for -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 1 -v");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Not a number for -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t x -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// < 0 for -t
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t -1 -v abc");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
try
{
// Invalid char for -v
Ice::ObjectPrx p = communicator->stringToProxy("id:opaque -t 99 -v x?c");
test(false);
}
catch(const Ice::EndpointParseException&)
{
}
// Legal TCP endpoint expressed as opaque endpoint
Ice::ObjectPrx p1 = communicator->stringToProxy("test:opaque -t 1 -v CTEyNy4wLjAuMeouAAAQJwAAAA==");
string pstr = communicator->proxyToString(p1);
test(pstr == "test -t:tcp -h 127.0.0.1 -p 12010 -t 10000");
// Working?
p1->ice_ping();
// Two legal TCP endpoints expressed as opaque endpoints
p1 = communicator->stringToProxy("test:opaque -t 1 -v CTEyNy4wLjAuMeouAAAQJwAAAA==:opaque -t 1 -v CTEyNy4wLjAuMusuAAAQJwAAAA==");
pstr = communicator->proxyToString(p1);
test(pstr == "test -t:tcp -h 127.0.0.1 -p 12010 -t 10000:tcp -h 127.0.0.2 -p 12011 -t 10000");
//
// Test that an SSL endpoint and a nonsense endpoint get written
// back out as an opaque endpoint.
//
p1 = communicator->stringToProxy("test:opaque -t 2 -v CTEyNy4wLjAuMREnAAD/////AA==:opaque -t 99 -v abch");
pstr = communicator->proxyToString(p1);
test(pstr == "test -t:opaque -t 2 -v CTEyNy4wLjAuMREnAAD/////AA==:opaque -t 99 -v abch");
//
// Try to invoke on the SSL endpoint to verify that we get a
// NoEndpointException.
//
try
{
p1->ice_ping();
test(false);
}
catch(const Ice::NoEndpointException&)
{
}
//
// Test that the proxy with an SSL endpoint and a nonsense
// endpoint (which the server doesn't understand either) can be
// sent over the wire and returned by the server without losing
// the opaque endpoints.
//
Ice::ObjectPrx p2 = derived->echo(p1);
pstr = communicator->proxyToString(p2);
test(pstr == "test -t:opaque -t 2 -v CTEyNy4wLjAuMREnAAD/////AA==:opaque -t 99 -v abch");
tprintf("ok\n");
#endif
return cl;
}
int main(int argc, char** argv) {
pthread_t thr_gui;
introrob::Control *control;
int status;
Ice::CommunicatorPtr ic;
struct timeval a, b;
long diff;
long totalb, totala;
bool guiActivated = 1;
bool controlActivated = 0;
//---------------- INPUT ARGUMENTS ---------------//
/*
if (argc!=3){
printf("\n");
printf("USE: ./introrob --Ice.Config=introrob.cfg OPTION\n");
printf(" -G to show the GUI\n");
printf(" -C to run IterationControl\n");
}
if((argc==3)&&(!strcmp(argv[2],"-G"))){
guiActivated=1;
controlActivated=0;
}
*/
if ((argc == 3) && (!strcmp(argv[2], "--nogui"))) {
controlActivated = 1;
guiActivated = 0;
}
api = new introrob::Api();
//api->showImage=true;
if ((argc == 3) && (!strcmp(argv[2], "-d"))) {
api->showImage = true;
}
//----------------END INPUT ARGUMENTS -----------------//
control = new introrob::Control();
//control->iterationControlActivated=false;
pthread_mutex_init(&api->controlGui, NULL);
try {
//-----------------ICE----------------//
ic = Ice::initialize(argc, argv);
// Contact to MOTORS interface
Ice::ObjectPrx baseMotors = ic->propertyToProxy("introrob.Motors.Proxy");
if (0 == baseMotors)
throw "Could not create proxy with motors";
// Cast to motors
control->mprx = jderobot::MotorsPrx::checkedCast(baseMotors);
if (0 == control->mprx)
throw "Invalid proxy introrob.Motors.Proxy";
// Get driver camera
Ice::ObjectPrx camara1 = ic->propertyToProxy("introrob.Camera1.Proxy");
if (0 == camara1)
throw "Could not create proxy to camera1 server";
// cast to CameraPrx
control->cprx1 = jderobot::CameraPrx::checkedCast(camara1);
if (0 == control->cprx1)
throw "Invalid proxy";
// Get driver camera
Ice::ObjectPrx camara2 = ic->propertyToProxy("introrob.Camera2.Proxy");
if (0 == camara2)
throw "Could not create proxy to camera2 server";
// cast to CameraPrx
control->cprx2 = jderobot::CameraPrx::checkedCast(camara2);
if (0 == control->cprx2)
throw "Invalid proxy";
// Contact to ENCODERS interface
Ice::ObjectPrx baseEncoders = ic->propertyToProxy("introrob.Encoders.Proxy");
if (0 == baseEncoders)
throw "Could not create proxy with encoders";
// Cast to encoders
control->eprx = jderobot::EncodersPrx::checkedCast(baseEncoders);
if (0 == control->eprx)
throw "Invalid proxy introrob.Encoders.Proxy";
// Contact to POSE3D interface
Ice::ObjectPrx basePose3D = ic->propertyToProxy("introrob.Pose3D.Proxy");
if (0 == basePose3D)
throw "Could not create proxy with pose3d";
// Cast to pose3d
control->p3dprx = jderobot::Pose3DPrx::checkedCast(basePose3D);
if (0 == control->p3dprx)
throw "Invalid proxy introrob.Pose3D.Proxy";
// Contact to LASER interface
Ice::ObjectPrx baseLaser = ic->propertyToProxy("introrob.Laser.Proxy");
if (0 == baseLaser)
throw "Could not create proxy with laser";
// Cast to laser
control->lprx = jderobot::LaserPrx::checkedCast(baseLaser);
if (0 == control->lprx)
throw "Invalid proxy introrob.Laser.Proxy";
// Contact to Pose3dEncoders interface
Ice::ObjectPrx pose3dencoders2 = ic->propertyToProxy("introrob.Pose3Dencoders2.Proxy");
if (0 == pose3dencoders2)
throw "Could not create proxy with encoders";
// Cast to encoders
control->p3deprx2 = jderobot::Pose3DEncodersPrx::checkedCast(pose3dencoders2);
if (0 == control->p3deprx2)
throw "Invalid proxy introrob.Pose3Dencoders2.Proxy";
// Contact to Pose3dEncoders interface
Ice::ObjectPrx pose3dencoders1 = ic->propertyToProxy("introrob.Pose3Dencoders1.Proxy");
if (0 == pose3dencoders1)
throw "Could not create proxy with encoders";
// Cast to encoders
control->p3deprx1 = jderobot::Pose3DEncodersPrx::checkedCast(pose3dencoders1);
if (0 == control->p3deprx1)
throw "Invalid proxy introrob.Pose3Dencoders1.Proxy";
// Contact to Pose3dEncoders interface
Ice::ObjectPrx pose3dmotors2 = ic->propertyToProxy("introrob.Pose3Dmotors2.Proxy");
if (0 == pose3dmotors2)
throw "Could not create proxy with encoders";
// Cast to encoders
control->p3dmprx2 = jderobot::Pose3DMotorsPrx::checkedCast(pose3dmotors2);
if (0 == control->p3dmprx2)
throw "Invalid proxy introrob.Pose3Dencoders2.Proxy";
// Contact to Pose3dEncoders interface
Ice::ObjectPrx pose3dmotors1 = ic->propertyToProxy("introrob.Pose3Dmotors1.Proxy");
if (0 == pose3dmotors1)
throw "Could not create proxy with encoders";
// Cast to encoders
control->p3dmprx1 = jderobot::Pose3DMotorsPrx::checkedCast(pose3dmotors1);
if (0 == control->p3dmprx1)
throw "Invalid proxy introrob.Pose3Dencoders1.Proxy";
//-----------------END ICE----------------//
//****************************** Processing the Control ******************************///
api->guiVisible = true;
api->sentido = 10;
api->accion = 0;
api->guiReady = FALSE;
api->imagesReady = FALSE;
control->UpdateSensorsICE(api);
if (guiActivated) {
pthread_create(&thr_gui, NULL, &showGui, NULL);
}
while (api->guiVisible) {
gettimeofday(&a, NULL);
totala = a.tv_sec * 1000000 + a.tv_usec;
control->UpdateSensorsICE(api); // Update sensors
if (controlActivated || api->iterationControlActivated) {
api->RunNavigationAlgorithm();
if (api->showImage)
api->showMyImage();
}
control->SetActuatorsICE(api); // Set actuators
//Sleep Algorithm
gettimeofday(&b, NULL);
totalb = b.tv_sec * 1000000 + b.tv_usec;
diff = (totalb - totala) / 1000;
if (diff < 0 || diff > cycle_control)
diff = cycle_control;
else
diff = cycle_control - diff;
/*Sleep Algorithm*/
usleep(diff * 1000);
if (diff < 33)
usleep(33 * 1000);
//printf("CONTROL %.15lf seconds elapsed\n", diff);
}
//****************************** END Processing the Control ******************************///
if (guiActivated)
pthread_join(thr_gui, NULL);
} catch (const Ice::Exception& ex) {
std::cerr << ex << std::endl;
status = 1;
} catch (const char* msg) {
std::cerr << msg << std::endl;
status = 1;
}
if (ic)
ic->destroy();
return 0;
}
Sensors::Sensors(Ice::CommunicatorPtr ic)
{
this-> ic = ic;
Ice::PropertiesPtr prop = ic->getProperties();
////////////////////////////// ENCODERS //////////////////////////////
// Contact to ENCODERS interface
Ice::ObjectPrx baseEncoders = ic->propertyToProxy("introrob.Encoders.Proxy");
if (0 == baseEncoders) {
encodersON = false;
std::cout << "Encoders configuration not specified" <<std::endl;
//throw "Could not create proxy with encoders";
}else{
// Cast to encoders
try {
eprx = jderobot::EncodersPrx::checkedCast(baseEncoders);
if (0 == eprx)
throw "Invalid proxy introrob.Encoders.Proxy";
encodersON = true;
std::cout << "Encoders connected" << std::endl;
}catch (Ice::ConnectionRefusedException& e){
encodersON=false;
std::cout << "Encoders inactive" << std::endl;
}
}
////////////////////////////// CAMERA1 /////////////////////////////2
jderobot::ImageDataPtr data;
Ice::ObjectPrx baseCamera1 = ic->propertyToProxy("introrob.Camera1.Proxy");
if (0==baseCamera1) {
camera1ON = false;
image1.create(400, 400, CV_8UC3);
std::cout << "Camera1 configuration not specified" <<std::endl;
//throw "Could not create proxy";
}else{
/*cast to CameraPrx*/
try {
camera1 = jderobot::CameraPrx::checkedCast(baseCamera1);
if (0==camera1)
throw "Invalid proxy";
camera1ON = true;
std::cout << "Camera1 connected" << std::endl;
data = camera1->getImageData(camera1->getImageFormat().at(0));
image1.create(data->description->height, data->description->width, CV_8UC3);
}catch (Ice::ConnectionRefusedException& e){
camera1ON=false;
std::cout << "Camera1 inactive" << std::endl;
//create an empty image if no camera connected (avoid seg. fault)
image1.create(400, 400, CV_8UC3);
}}
////////////////////////////// CAMERA2 /////////////////////////////2
Ice::ObjectPrx baseCamera2 = ic->propertyToProxy("introrob.Camera2.Proxy");
if (0==baseCamera2) {
camera2ON = false;
image2.create(400, 400, CV_8UC3);
std::cout << "Camera2 configuration not specified" <<std::endl;
//throw "Could not create proxy";
}else{
/*cast to CameraPrx*/
try {
camera2 = jderobot::CameraPrx::checkedCast(baseCamera2);
if (0==camera2)
throw "Invalid proxy";
camera2ON = true;
std::cout << "Camera2 connected" << std::endl;
data = camera2->getImageData(camera2->getImageFormat().at(0));
image2.create(data->description->height, data->description->width, CV_8UC3);
}catch (Ice::ConnectionRefusedException& e){
camera2ON=false;
std::cout << "Camera2 inactive" << std::endl;
//create an empty image if no camera connected (avoid seg. fault)
image2.create(400, 400, CV_8UC3);
}}
////////////////////////////// LASER //////////////////////////////
// Contact to LASER interface
Ice::ObjectPrx laserICE = ic->propertyToProxy("introrob.Laser.Proxy");
if (0 == laserICE) {
laserON = false;
std::cout << "Laser configuration not specified" <<std::endl;
//throw "Could not create proxy with Laser";
}else{
// Cast to LASER
try {
laserprx = jderobot::LaserPrx::checkedCast(laserICE);
if (0 == laserprx){
throw std::string("Invalid proxy introrob.Laser.Proxy");
}
laserON = true;
std::cout << "Laser connected" << std::endl;
}catch (Ice::ConnectionRefusedException& e){
laserON=false;
std::cout << "Laser inactive" << std::endl;
}}
/*boolLaser = prop->getPropertyAsInt("introrob.Laser");
std::cout << "Laser " << boolLaser << std::endl;
if(boolLaser){
// Contact to LASER interface
Ice::ObjectPrx laserICE = ic->propertyToProxy("introrob.Laser.Proxy");
if (0 == laserICE)
throw "Could not create proxy with Laser";
// Cast to LASER
laserprx = jderobot::LaserPrx::checkedCast(laserICE);
if (0 == laserprx){
throw std::string("Invalid proxy introrob.Laser.Proxy");
}
}*/
}
Sensors::Sensors(Ice::CommunicatorPtr ic)
{
this-> ic = ic;
Ice::PropertiesPtr prop = ic->getProperties();
////////////////////////////// ENCODERS //////////////////////////////
// Contact to ENCODERS interface
Ice::ObjectPrx baseEncoders = ic->propertyToProxy("introrob.Encoders.Proxy");
if (0 == baseEncoders)
throw "Could not create proxy with encoders";
// Cast to encoders
eprx = jderobot::EncodersPrx::checkedCast(baseEncoders);
if (0 == eprx)
throw "Invalid proxy introrob.Encoders.Proxy";
////////////////////////////// CAMERA1 /////////////////////////////2
Ice::ObjectPrx baseCamera1 = ic->propertyToProxy("introrob.Camera1.Proxy");
if (0==baseCamera1)
throw "Could not create proxy";
/*cast to CameraPrx*/
camera1 = jderobot::CameraPrx::checkedCast(baseCamera1);
if (0==camera1)
throw "Invalid proxy";
jderobot::ImageDataPtr data = camera1->getImageData(camera1->getImageFormat().at(0));
image1.create(data->description->height, data->description->width, CV_8UC3);
////////////////////////////// CAMERA2 /////////////////////////////2
Ice::ObjectPrx baseCamera2 = ic->propertyToProxy("introrob.Camera2.Proxy");
if (0==baseCamera2)
throw "Could not create proxy";
/*cast to CameraPrx*/
camera2 = jderobot::CameraPrx::checkedCast(baseCamera2);
if (0==camera2)
throw "Invalid proxy";
data = camera2->getImageData(camera2->getImageFormat().at(0));
image2.create(data->description->height, data->description->width, CV_8UC3);
////////////////////////////// LASER //////////////////////////////
boolLaser = prop->getPropertyAsInt("introrob.Laser");
std::cout << "Laser " << boolLaser << std::endl;
if(boolLaser){
// Contact to LASER interface
Ice::ObjectPrx laserICE = ic->propertyToProxy("introrob.Laser.Proxy");
if (0 == laserICE)
throw "Could not create proxy with Laser";
// Cast to LASER
laserprx = jderobot::LaserPrx::checkedCast(laserICE);
if (0 == laserprx){
throw std::string("Invalid proxy introrob.Laser.Proxy");
}
}
}
Control::Control(Ice::CommunicatorPtr ic, Model* sm) {
/*Obtaining the configuration file (*.cfg) properties such as ports and IP's*/
this->ic = ic;
this->sm = sm;
Ice::PropertiesPtr prop = ic->getProperties();
// *** camRGB ***
/*Checking if the property has value and the creation of the proxy is possible. If the property is not set or has no value
it will be set as "miss" and then we will know that we cannot create a proxy with the camera (or other sensors/actuators)*/
std::string camRGBCfg = prop->getPropertyWithDefault("real_rt_estimator.CameraRGB.Proxy", "miss");
if (!boost::iequals(camRGBCfg , "miss")) {
/*Creation of a proxy to connect with cameraServer*/
Ice::ObjectPrx baseRGB = ic->propertyToProxy("real_rt_estimator.CameraRGB.Proxy");
if (0==baseRGB)
throw "Could not create proxy";
// parallelIce for camRGB
camRGB = new jderobot::cameraClient(ic, "real_rt_estimator.CameraRGB.");
if (camRGB != NULL){
cameraRGBOn = true;
camRGB->start();
//create_gui=true;
} else {
cameraRGBOn = false;
throw "Failed to load RGB Camera";
}
cv::Size rgbSize(0,0);
cv::Mat data;
camRGB->getImage(data,true);
rgbSize=data.size();
/*Create the first image obtained from the camera and stores in the shared memory*/
this->sm->createImageRGB(data);
} else {
cameraRGBOn = false;
/*Create an empty image if there is no camera connected*/
//this->sm->createEmptyImageRGB();
std::cout << "No camera RGB connected." << std::endl;
}
// *** camDEPTH ***
std::string camDEPTHCfg = prop->getPropertyWithDefault("real_rt_estimator.CameraDEPTH.Proxy", "miss");
if (!boost::iequals(camDEPTHCfg , "miss")) {
/*Creation of a proxy to connect with cameraServer*/
Ice::ObjectPrx baseDEPTH = ic->propertyToProxy("real_rt_estimator.CameraDEPTH.Proxy");
if (0==baseDEPTH)
throw "Could not create proxy";
// parallelIce for camDEPTH
camDEPTH = new jderobot::cameraClient(ic, "real_rt_estimator.CameraDEPTH.");
if (camDEPTH != NULL){
cameraDEPTHOn = true;
camDEPTH->start();
//create_gui=true;
} else {
cameraDEPTHOn = false;
throw "Failed to load DEPTH Camera";
}
cv::Size depthSize(0,0);
cv::Mat dataDEPTH;
camDEPTH->getImage(dataDEPTH,true);
depthSize=dataDEPTH.size();
/*Create the first image obtained from the camera and stores in the shared memory*/
this->sm->createImageDEPTH(dataDEPTH);
} else {
cameraDEPTHOn = false;
/*Create an empty image if there is no camera connected*/
//this->sm->createEmptyImageDEPTH();
std::cout << "No camera DEPTH connected" << std::endl;
}
//Let's check if the user want to show the gui or not. This setting must be in the .cfg file
std::string gui = prop->getPropertyWithDefault("real_rt_estimator.Gui", "miss");
if (!boost::iequals(gui , "miss") && !boost::iequals(gui, "OFF")) {
} else {
std::cout << "No Gui mode -> automatic control mode" << std::endl;
automaticMode = true;
}
}