[Jderobot-admin] jderobot-r868 - trunk/src/components/pluginsGazebo/kinect
ahcorde en jderobot.org
ahcorde en jderobot.org
Vie Mar 1 10:34:40 CET 2013
Author: ahcorde
Date: 2013-03-01 10:33:40 +0100 (Fri, 01 Mar 2013)
New Revision: 868
Modified:
trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc
trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h
Log:
[ahcorde] Actualizado el plugin de Kinect para gazeboserver
Modified: trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc 2013-02-28 22:43:53 UTC (rev 867)
+++ trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc 2013-03-01 09:33:40 UTC (rev 868)
@@ -23,6 +23,8 @@
using namespace gazebo;
+gazebo::DepthCameraPlugin* kinect;
+
GZ_REGISTER_SENSOR_PLUGIN(DepthCameraPlugin)
void *mainKinect(void* v);
@@ -34,6 +36,7 @@
count = 0;
pthread_mutex_init (&mutex, NULL);
pthread_mutex_init (&mutexRGB, NULL);
+ gzerr << "Kinect Construtor\n";
}
@@ -65,12 +68,13 @@
this, _1, _2, _3, _4, _5));
+ /*
this->newRGBPointCloudConnection = this->depthCamera->ConnectNewRGBPointCloud(
boost::bind(&DepthCameraPlugin::OnNewRGBPointCloud,
this, _1, _2, _3, _4, _5));
+*/
-
this->parentSensor->SetActive(true);
}
@@ -127,13 +131,19 @@
unsigned int _width, unsigned int _height,
unsigned int _depth, const std::string &_format)
{
+ cycle = 100;
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
if(count == 0){
count++;
std::string name = this->parentSensor->GetName();
nameKinect = std::string("--Ice.Config=" + name + ".cfg");
pthread_t thr_gui;
pthread_create(&thr_gui, NULL, &mainKinect, (void*)this);
+ gzerr << "Creating Kinect\n";
+ cloud->points.resize(_width*_height);
}
double hfov = 1.04719755;
@@ -141,7 +151,7 @@
double pointCloudCutoff = 0.001;
//std::cout << "depth: " << (int)(_image[0]*1000) << std::endl;
-
+/*
cv::Mat image;
image.create(_height, _width, CV_32FC1);
@@ -153,14 +163,23 @@
cv::cvtColor(imageDepth, imageDepth, CV_GRAY2RGB);
//cv::imshow("profundidad", imageDepth);
//cv::waitKey(10);
+*/
-
double pAngle;
double yAngle;
- cloud->clear();
- for(int x = 0 ; x < _width ; x+=1){
- for(int y = 0; y < _height; y+=1){
+
+ pthread_mutex_lock (&kinect->mutex);
+ //cloud->points.resize(_width*_height);
+
+ int indicePunto = 0;
+ pcl::PointXYZRGBA point;
+ point.r = 255;
+ point.g = 0;
+ point.b = 0;
+
+ for(int x = 0 ; x < _width ; x++){
+ for(int y = 0; y < _height; y++){
int indice = y*_width + x;
if (_height>1)
@@ -175,12 +194,10 @@
float d = _image[indice];
- pcl::PointXYZRGBA point;
+
point.x = d * tan(yAngle);
point.y = d * tan(pAngle);
- point.r = 255;
- point.g = 0;
- point.b = 0;
+
if(d > pointCloudCutoff){
point.z = _image[indice];
}else{ //point in the unseeable range
@@ -189,25 +206,40 @@
cloud->is_dense = false;
}
- cloud->points.push_back(point);
+ cloud->points[indicePunto++] = point;
}
}
+
pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
sor.setInputCloud (cloud);
- sor.setLeafSize (0.02, 0.02, 0.02);
+ sor.setLeafSize (0.05, 0.05, 0.05);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGBA>);
sor.filter (*cloud2);
*cloud = *cloud2;
+
+ pthread_mutex_unlock (&kinect->mutex);
+
+ //pthread_mutex_unlock (&mutex);
- pthread_mutex_unlock (&mutex);
-
//std::cout << "cloud: " << cloud->points.size() << std::endl;
//if (!viewer.wasStopped())
// viewer.showCloud(cloud);
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ sleep(diff/1000);
+
}
/////////////////////////////////////////////////
@@ -215,9 +247,22 @@
unsigned int _width, unsigned int _height,
unsigned int _depth, const std::string &/*_format*/)
{
-/*
- std::cout << "OnNewRGBPointCloud Format: " << format << std::endl;
-*/
+ cycle3 = 100;
+ gettimeofday(&a3, NULL);
+ totala3 = a3.tv_sec * 1000000 + a3.tv_usec;
+
+ gettimeofday(&b3, NULL);
+ totalb3 = b3.tv_sec * 1000000 + b3.tv_usec;
+
+ diff3 = (totalb3 - totala3) / 1000;
+ diff3 = cycle3 - diff3;
+
+ if (diff3 < 10)
+ diff3 = 10;
+
+ sleep(diff3/1000);
+
+
}
/////////////////////////////////////////////////
@@ -227,6 +272,11 @@
unsigned int _depth,
const std::string &_format)
{
+
+ cycle2 = 100;
+ gettimeofday(&a2, NULL);
+ totala2 = a2.tv_sec * 1000000 + a2.tv_usec;
+
//std::cout << "OnNewImageFrame Format: " << format << " Depth: " << _depth << std::endl;
pthread_mutex_lock (&mutexRGB);
imageRGB.create(_height, _width, CV_8UC3);
@@ -234,7 +284,18 @@
//cv::imshow("Color", imageRGB);
//cv::waitKey(10);
pthread_mutex_unlock (&mutexRGB);
+
+ gettimeofday(&b2, NULL);
+ totalb2 = b2.tv_sec * 1000000 + b2.tv_usec;
+ diff2 = (totalb2 - totala2) / 1000;
+ diff2 = cycle2 - diff2;
+
+ if (diff2 < 10)
+ diff2 = 10;
+
+ sleep(diff2/1000);
+
}
class KinectI: virtual public jderobot::pointCloud{
@@ -250,6 +311,7 @@
pthread_mutex_lock (&kinect->mutex);
pcl::PointCloud<pcl::PointXYZRGBA> cloud = *kinect->cloud;
pthread_mutex_unlock (&kinect->mutex);
+
if(cloud.points.size()){
KData->p.resize(cloud.points.size());
int index = 0;
@@ -263,7 +325,7 @@
index++;
}
}
- return KData;
+ return KData;
};
jderobot::pointCloudDataPtr KData;
gazebo::DepthCameraPlugin* kinect;
@@ -576,7 +638,7 @@
void *mainKinect(void* v)
{
- gazebo::DepthCameraPlugin* kinect = (gazebo::DepthCameraPlugin*)v;
+ kinect = (gazebo::DepthCameraPlugin*)v;
char* name = (char*)kinect->nameKinect.c_str();
@@ -599,12 +661,12 @@
jderobotice::Context context;
Ice::ObjectPtr object = new KinectI(std::string("KinectGazebo"), context, kinect);
- Ice::ObjectPtr object2 = new CameraI(std::string("KinectGazebo"), context, kinect);
- Ice::ObjectPtr object3 = new CameraII(std::string("KinectGazebo"), context, kinect);
+ //Ice::ObjectPtr object2 = new CameraI(std::string("KinectGazebo"), context, kinect);
+ //Ice::ObjectPtr object3 = new CameraII(std::string("KinectGazebo"), context, kinect);
adapter->add(object, ic->stringToIdentity("Kinect"));
- adapter->add(object2, ic->stringToIdentity("KinectRGB"));
- adapter->add(object3, ic->stringToIdentity("KinectDepth"));
+ //adapter->add(object2, ic->stringToIdentity("KinectRGB"));
+ //adapter->add(object3, ic->stringToIdentity("KinectDepth"));
std::cout << " adapter->add(object, ic->stringToIdentity(Kinect)); " << std::endl;
adapter->activate();
ic->waitForShutdown();
Modified: trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h 2013-02-28 22:43:53 UTC (rev 867)
+++ trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h 2013-03-01 09:33:40 UTC (rev 868)
@@ -96,6 +96,18 @@
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud;
cv::Mat imageRGB;
cv::Mat imageDepth;
+
+ int cycle;
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ int cycle2;
+ long totalb2, totala2, diff2;
+ struct timeval a2, b2;
+
+ int cycle3;
+ long totalb3, totala3, diff3;
+ struct timeval a3, b3;
};
}
#endif
More information about the Jderobot-admin
mailing list