void targetSizeCb( int val, void *dummy )
{
std_msgs::Float32 msg;
msg.data = val;
targetSizePub.publish(msg);
}
void hRangeCb( int val, void *dummy )
{
std_msgs::Int32 msg;
msg.data = val;
hRangePub.publish( msg );
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// Now, the frame is in "cv_ptr->image"
std::vector<Rect> faces;
Mat frame_gray;
Mat frame(cv_ptr->image);
resize(frame, frame, Size(160, 120));
cvtColor( frame, frame_gray, COLOR_BGR2GRAY );
equalizeHist( frame_gray, frame_gray );
//t = (double)cvGetTickCount();
//-- Detect faces
//face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CASCADE_SCALE_IMAGE, Size(16, 12), Size(80, 60) ); // 30ms
face_cascade.detectMultiScale( frame_gray, faces, 1.05, 2); // 35ms
//t = (double)cvGetTickCount() - t;
//printf( "detection time = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );
if (faces.size() > 0) {
//myFilter((faces[0].x + faces[0].width/2) * 4 - 320, (faces[0].y + faces[0].height/2) * 4 - 240);
//firstBodyFound = 1;
before_filter_x = (faces[0].x + faces[0].width/2) * 4 - 320;
before_filter_y = (faces[0].y + faces[0].height/2) * 4 - 240;
}
else
{
//myFilter(0.0, 0.0);
before_filter_x = 0.0;
before_filter_y = 0.0;
}
myFilter(before_filter_x, before_filter_y);
gettimeofday(&tp, NULL);
now = tp.tv_sec * 1000000 + tp.tv_usec;
double t = (double) (now - start_time) / 1000000;
printf("%f %f %f\n", t, before_filter_x, filter_x);
if (1) {
size_t i = 0;
Point center(filter_x + 320.0, filter_y + 240);
cv::circle(cv_ptr->image, center, 50, CV_RGB(0,255,255), 5.0);
/*
rectangle(cv_ptr->image,
Point(filter_x, filter_y),
Point(filter_x + filter_width, filter_y + filter_height),
Scalar(0, 255, 255),
5,
8);
*/
// publishing
brray.data[0] = 0.0; // x: 0.0 (depth)
brray.data[1] = -filter_x; // y: -320 ~ +320
brray.data[2] = -filter_y; // z: -240 ~ +240
brray.data[3] = 0.0;
brray.data[4] = 0.0;
target_xywh_pub_.publish(brray);
// control base to head to the target
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
cmd->angular.z = -brray.data[1]/320 * 1.5;
cmd_pub_.publish(cmd);
}
// Draw an example circle on the video stream
//if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
// cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
// Update GUI Window
//cv::imshow(OPENCV_WINDOW, cv_ptr->image);
//cv::waitKey(3);
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
}
void morphSizeCb( int val, void *dummy )
{
geometry_msgs::Point msg;
msg.x = msg.y = val;
morphSizePub.publish(msg);
}
int my_callback(int data_type, int data_len, char *content)
{
g_lock.enter();
/* image data */
if (e_image == data_type && NULL != content)
{
image_data* data = (image_data*)content;
if ( data->m_greyscale_image_left[CAMERA_ID] ) {
memcpy(g_greyscale_image_left.data, data->m_greyscale_image_left[CAMERA_ID], IMAGE_SIZE);
imshow("left", g_greyscale_image_left);
// publish left greyscale image
cv_bridge::CvImage left_8;
g_greyscale_image_left.copyTo(left_8.image);
left_8.header.frame_id = "guidance";
left_8.header.stamp = ros::Time::now();
left_8.encoding = sensor_msgs::image_encodings::MONO8;
left_image_pub.publish(left_8.toImageMsg());
}
if ( data->m_greyscale_image_right[CAMERA_ID] ) {
memcpy(g_greyscale_image_right.data, data->m_greyscale_image_right[CAMERA_ID], IMAGE_SIZE);
imshow("right", g_greyscale_image_right);
// publish right greyscale image
cv_bridge::CvImage right_8;
g_greyscale_image_right.copyTo(right_8.image);
right_8.header.frame_id = "guidance";
right_8.header.stamp = ros::Time::now();
right_8.encoding = sensor_msgs::image_encodings::MONO8;
right_image_pub.publish(right_8.toImageMsg());
}
if ( data->m_depth_image[CAMERA_ID] ) {
memcpy(g_depth.data, data->m_depth_image[CAMERA_ID], IMAGE_SIZE * 2);
g_depth.convertTo(depth8, CV_8UC1);
imshow("depth", depth8);
//publish depth image
cv_bridge::CvImage depth_16;
g_depth.copyTo(depth_16.image);
depth_16.header.frame_id = "guidance";
depth_16.header.stamp = ros::Time::now();
depth_16.encoding = sensor_msgs::image_encodings::MONO16;
depth_image_pub.publish(depth_16.toImageMsg());
}
key = waitKey(1);
}
/* imu */
if ( e_imu == data_type && NULL != content )
{
imu *imu_data = (imu*)content;
printf( "frame index: %d, stamp: %d\n", imu_data->frame_index, imu_data->time_stamp );
printf( "imu: [%f %f %f %f %f %f %f]\n", imu_data->acc_x, imu_data->acc_y, imu_data->acc_z, imu_data->q[0], imu_data->q[1], imu_data->q[2], imu_data->q[3] );
// publish imu data
geometry_msgs::TransformStamped g_imu;
g_imu.header.frame_id = "guidance";
g_imu.header.stamp = ros::Time::now();
g_imu.transform.translation.x = imu_data->acc_x;
g_imu.transform.translation.y = imu_data->acc_y;
g_imu.transform.translation.z = imu_data->acc_z;
g_imu.transform.rotation.w = imu_data->q[0];
g_imu.transform.rotation.x = imu_data->q[1];
g_imu.transform.rotation.y = imu_data->q[2];
g_imu.transform.rotation.z = imu_data->q[3];
imu_pub.publish(g_imu);
}
/* velocity */
if ( e_velocity == data_type && NULL != content )
{
velocity *vo = (velocity*)content;
printf( "frame index: %d, stamp: %d\n", vo->frame_index, vo->time_stamp );
printf( "vx:%f vy:%f vz:%f\n", 0.001f * vo->vx, 0.001f * vo->vy, 0.001f * vo->vz );
// publish velocity
geometry_msgs::Vector3Stamped g_vo;
g_vo.header.frame_id = "guidance";
g_vo.header.stamp = ros::Time::now();
g_vo.vector.x = 0.001f * vo->vx;
g_vo.vector.y = 0.001f * vo->vy;
g_vo.vector.z = 0.001f * vo->vz;
velocity_pub.publish(g_vo);
}
/* obstacle distance */
if ( e_obstacle_distance == data_type && NULL != content )
{
obstacle_distance *oa = (obstacle_distance*)content;
printf( "frame index: %d, stamp: %d\n", oa->frame_index, oa->time_stamp );
printf( "obstacle distance:" );
for ( int i = 0; i < CAMERA_PAIR_NUM; ++i )
{
printf( " %f ", 0.01f * oa->distance[i] );
}
printf( "\n" );
// publish obstacle distance
sensor_msgs::LaserScan g_oa;
g_oa.ranges.resize(CAMERA_PAIR_NUM);
g_oa.header.frame_id = "guidance";
g_oa.header.stamp = ros::Time::now();
for ( int i = 0; i < CAMERA_PAIR_NUM; ++i )
g_oa.ranges[i] = 0.01f * oa->distance[i];
obstacle_distance_pub.publish(g_oa);
}
/* ultrasonic */
if ( e_ultrasonic == data_type && NULL != content )
{
ultrasonic_data *ultrasonic = (ultrasonic_data*)content;
printf( "frame index: %d, stamp: %d\n", ultrasonic->frame_index, ultrasonic->time_stamp );
for ( int d = 0; d < CAMERA_PAIR_NUM; ++d )
{
printf( "ultrasonic distance: %f, reliability: %d\n", ultrasonic->ultrasonic[d] * 0.001f, (int)ultrasonic->reliability[d] );
}
// publish ultrasonic data
sensor_msgs::LaserScan g_ul;
g_ul.ranges.resize(CAMERA_PAIR_NUM);
g_ul.intensities.resize(CAMERA_PAIR_NUM);
g_ul.header.frame_id = "guidance";
g_ul.header.stamp = ros::Time::now();
for ( int d = 0; d < CAMERA_PAIR_NUM; ++d ) {
g_ul.ranges[d] = 0.001f * ultrasonic->ultrasonic[d];
g_ul.intensities[d] = 1.0 * ultrasonic->reliability[d];
}
ultrasonic_pub.publish(g_ul);
}
if(e_motion == data_type && NULL!=content) {
motion* m=(motion*)content;
printf("frame index: %d, stamp: %d\n", m->frame_index, m->time_stamp);
printf("(px,py,pz)=(%.2f,%.2f,%.2f)\n", m->position_in_global_x, m->position_in_global_y, m->position_in_global_z);
// publish position
geometry_msgs::Vector3Stamped g_pos;
g_pos.header.frame_id = "guidance";
g_pos.header.stamp = ros::Time::now();
g_pos.vector.x = m->position_in_global_x;
g_pos.vector.y = m->position_in_global_y;
g_pos.vector.z = m->position_in_global_z;
position_pub.publish(g_pos);
}
g_lock.leave();
g_event.set_event();
return 0;
}
int getNumRGBSubscribers()
{
return realsense_reg_points_pub.getNumSubscribers() + realsense_rgb_image_pub.getNumSubscribers();
}
void tell_want_to_dance()
{
int sentence_num = rand() % NUM_OF_SENTENCES;
etts_msg.text = sentences_to_dance[sentence_num];
etts_say_text.publish(etts_msg);
}
void velocityCallback(const gen2_motor_driver::pid_plot &pid_plot)
{
//Calculate the time between messages for the wheel velocity function.
last_time = current_time;
current_time = ros::Time::now();
double vx = ((pid_plot.left_vel/100)+(pid_plot.right_vel/100))/2;
double vy = 0;
double vth = ((pid_plot.right_vel/100)-(pid_plot.left_vel/100))/(wheel_base/100);
//compute odometry in a typical way given the velocities of the robot
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += (delta_th * encoder_scale_correction);
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
//first, we'll publish the transform over tf
/*odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;*/
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
boost::array<double, 36> covariance = {1e-10, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.001, 1e-10, 0.0, 0.0, 0.0,
0.0, 0.0, 1e6, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e6, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e6, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1e-10};
odom.pose.covariance = covariance;
odom.twist.covariance = covariance;
//set the velocity
odom.child_frame_id = "base_footprint";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;
//publish the message
odom_pub.publish(odom);
}
void PlaneFinder::pcCallback(const sensor_msgs::PointCloud2::ConstPtr & pc)
{
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered3 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
sensor_msgs::PointCloud2 pc2;
double height = -0.5;
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL(*(pc), *(pcl_pc));
//////////////////
// Voxel filter //
//////////////////
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (pcl_pc);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_filtered), pc2);
//debug2_pub.publish(pc2);
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(cloud_filtered), *(cloud_filtered1));
if(pcl_ros::transformPointCloud("map", *(cloud_filtered1), *(cloud_filtered2), tf_))
{
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.003, 0.9);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
/////////////////////////
// Planar segmentation //
/////////////////////////
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
//seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
int i = 0, nr_points = (int) cloud_filtered2->points.size ();
// While 50% of the original cloud is still there
while (cloud_filtered2->points.size () > 0.5 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered2);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
if( (fabs(coefficients->values[0]) < 0.02) &&
(fabs(coefficients->values[1]) < 0.02) &&
(fabs(coefficients->values[2]) > 0.9) )
{
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
height = coefficients->values[3];
// Extract the inliers
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_filtered3);
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered3), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug_pub.publish(pc2);
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered2.swap (cloud_f);
i++;
}
/*
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("z");
pass.setFilterLimits (height, 1.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
*/
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug_pub.publish(pc2);
/////////////////////////////////
// Statistical Outlier Removal //
/////////////////////////////////
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_filtered2);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered2);
//////////////////////////////////
// Euclidian Cluster Extraction //
//////////////////////////////////
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud_filtered2);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (5000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered2);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster1 (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster1->points.push_back (cloud_filtered2->points[*pit]);
cloud_cluster1->width = cloud_cluster1->points.size ();
cloud_cluster1->height = 1;
cloud_cluster1->is_dense = true;
cloud_cluster1->header.frame_id = "/map";
if(j == 0) {
// Transformation into ROS type
pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
debug_pub.publish(pc2);
}
else {
// Transformation into ROS type
pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
debug2_pub.publish(pc2);
}
j++;
}
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug2_pub.publish(pc2);
//debug2_pub.publish(*pc_map);
}
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
//cloud definition
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud2;
pcl::PointCloud<pcl::PointXYZ> cloud3;
pcl::ExtractIndices<pcl::PointXYZ> extract;
pcl::PointCloud<pcl::PointXYZ> obj_cloud1;
pcl::PointCloud<pcl::PointXYZ> cloud_objetos;
//sensor definition
sensor_msgs::PointCloud2 Plano_suelo;
sensor_msgs::PointCloud2 cloud_sobrante;
sensor_msgs::PointCloud2 Objeto_cercano;
sensor_msgs::PointCloud2 Objetos_detectados;
Fitrar_Voxel(input, 0.008);
if (cloud_voxel.height*cloud_voxel.width>9000){
if (error==true){
std::cerr << "DETECTANDO PUNTOS " << cloud_voxel.height*cloud_voxel.width<< std::endl;
error=false;
}
pcl::fromROSMsg (cloud_voxel, cloud);
// segmentacion plana
pcl::ModelCoefficients coefficients;
pcl::PointIndices inliers;
float anguloK_S;
while(true){
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setMaxIterations (200);
seg.setInputCloud (cloud.makeShared ());
seg.segment (inliers, coefficients);
//calculo del angulo de la kinect (angulo formado por la kinect y el suelo)
anguloK_S=acos(-coefficients.values[1]/sqrt(pow(coefficients.values[2],2)+pow(-coefficients.values[0],2)+pow(-coefficients.values[1],2)))-PI;
if (anguloK_S*180/PI>-75){
//std::cerr << "Angulo kinect/suelo: " << anguloK_S*180/PI << std::endl;
break;
}
else{
//std::cerr << "Angulo kinect/suelo: " << anguloK_S*180/PI << "Recalculo del plan " << std::endl;
extract.setInputCloud (cloud.makeShared ());
extract.setIndices (boost::make_shared<pcl::PointIndices> (inliers));
extract.setNegative (true);
extract.filter (cloud);
}
}
/*std::cerr << "Model coefficients: " << coefficients.values[0] << " "
<< coefficients.values[1] << " "
<< coefficients.values[2] << " "
<< coefficients.values[3] << std::endl;
std::cerr << "Model inliers: " << inliers.indices.size () << std::endl;*/
pcl::copyPointCloud(cloud, inliers, cloud2);
cloud3=cloud;
extract.setInputCloud (cloud.makeShared ());
extract.setIndices (boost::make_shared<pcl::PointIndices> (inliers));
extract.setNegative (true);
extract.filter (cloud3);
// Creating the KdTree object for the search method of the extraction
//Segmentacion por agrupacion
pcl::search::KdTree<pcl::PointXYZ> tree;
tree.setInputCloud (cloud3.makeShared ());
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (80);
ec.setMaxClusterSize (1500);
ec.setSearchMethod (boost::make_shared<pcl::search::KdTree<pcl::PointXYZ> > (tree));
ec.setInputCloud (cloud3.makeShared ());
ec.extract (cluster_indices);
//std::cerr << "Numero de objecto detectado " << cluster_indices.size() << std::endl;
// Examine the clusters and find the nearest centroid
float min_d = 10000;
int memorize_indice=0;
Eigen::Vector4f close;
cloud_objetos.header=cloud3.header;
for (unsigned int i = 0; i < cluster_indices.size(); ++i){
pcl::PointCloud<pcl::PointXYZ> obj_cloud;
pcl::copyPointCloud(cloud3, cluster_indices[i], obj_cloud);
cloud_objetos+=obj_cloud;
Eigen::Vector4f xyz_centroid;
pcl::compute3DCentroid(obj_cloud, xyz_centroid);
if (sqrt(pow(xyz_centroid[0],2)+pow(xyz_centroid[1],2)+ pow(xyz_centroid[2],2))< min_d){
min_d = sqrt(pow(xyz_centroid[0],2)+pow(xyz_centroid[1],2)+ pow(xyz_centroid[2],2));
close [0]=xyz_centroid[2];
close [1]=-xyz_centroid[0];
close [2]=-xyz_centroid[1];
memorize_indice=i;
}
}
//std::cerr << "Distancia minima " << min_d << std::endl;
/*std::cerr << "Nearest " << close[0] << ","
<< close[1] << ","
<< close[2] << std::endl;*/
//Despues de la matriz de rotacion
Eigen::Vector4f close_Robot;
close_Robot[0]=(close[0]*cos(-anguloK_S)+close[2]*sin(-anguloK_S))*1000-20;
close_Robot[1]=(close[1])*1000;
close_Robot[2]=(-close[0]*sin(-anguloK_S)+close[2]*cos(-anguloK_S))*1000+360;
int sizeMax=0;
if(cluster_indices.size()>0){
pcl::copyPointCloud(cloud3, cluster_indices[memorize_indice], obj_cloud1);
pcl::toROSMsg(obj_cloud1, Objeto_cercano);
sizeMax=1;
}
Centroide.x=close_Robot[0];
Centroide.y=close_Robot[1];
Centroide.z=close_Robot[2];
//std::cerr << "Nearest en Base_r "<<Centroide.x<<" "<< Centroide.y<<" "<<Centroide.z << std::endl;
//std::cerr << "Puntos del objeto " << obj_cloud1.size() << std::endl;
//Convert the pcl cloud back to rosmsg
pcl::toROSMsg(cloud2, Plano_suelo);
pcl::toROSMsg(cloud3, cloud_sobrante);
pcl::toROSMsg(cloud_objetos, Objetos_detectados);
//Set the header of the cloud
//cloud_filtered.header.frame_id = cloud.header.frame_id;
cloud_sobrante.header.frame_id = cloud.header.frame_id;
// Publish the data
pub_Objet.publish(Centroide);
pub_Plano_suelo.publish (Plano_suelo);
pub_cloud_sobrante.publish (cloud_sobrante);
pub_Objeto_cercano.publish (Objeto_cercano);
pub_Objetos_detectados.publish (Objetos_detectados);
}
else{
if (error==false){
std::cerr << "ERROR!! NUMERO DE PUNTOS INSUFICIANTE PARA CALCULOS!! ESPERANDO MAS PUNTOS " << std::endl;
error=true;
}
}
}
void VisualizationPublisherCCD::visualizationduringmotion(){
R_point *temp_point;
int temp_length;
robotpath.points.clear();
toolpath.points.clear();
// mine.points.clear();
// coilpath.points.clear();
visitedpath.points.clear();
int metric=M->metric;
if(CCD->getPathLength()>0){
temp_length=CCD->getPathLength();
temp_point=CCD->GetRealPathRobot();
geometry_msgs::Point p;
for(int pathLength=0; pathLength<temp_length;pathLength++){
p.x = temp_point[pathLength].x/metric;
p.y = temp_point[pathLength].y/metric;
p.z = 0;
robotpath.points.push_back(p);
}
//publish path
robotpath_pub.publish(robotpath);
}
if(CCD->getPathLength()>0){
temp_length=CCD->getPathLength();
temp_point=CCD->GetRealPathTool();
geometry_msgs::Point p;
for(int pathLength=0; pathLength<temp_length;pathLength++){
p.x = temp_point[pathLength].x/metric;
p.y = temp_point[pathLength].y/metric;
p.z = 0;
toolpath.points.push_back(p);
}
//publish path
toolpath_pub.publish(toolpath);
}
if(CCD->getPathLength()>0){
geometry_msgs::Point p;
for (int i=0; i<CCD->MapSizeX; i++){
for (int j=0; j<CCD->MapSizeY; j++){
if ((CCD->map[i][j].presao>0)){
p.x=(GM->Map_Home.x+i*GM->Map_Cell_Size+0.5*GM->Map_Cell_Size)/metric;
p.y=(GM->Map_Home.y+j*GM->Map_Cell_Size+0.5*GM->Map_Cell_Size)/metric;
visitedpath.points.push_back(p);
}
}
}
visitedpath_pub.publish(visitedpath);
}
#if 0
if(CCD->coilpeak){
geometry_msgs::Point p;
p.x = CCD->tocka.x;
p.y = CCD->tocka.y;
p.z = 0;
coilpath.points.push_back(p);
p.x = CCD->tockaR.x;
p.y = CCD->tockaR.y;
p.z = 0;
coilpath.points.push_back(p);
//publish path
coil_pub.publish(coilpath);
}
if (0)
{
geometry_msgs::Point p;
p.x = 3.;
p.y = 0;
p.z = 0;
mine.points.push_back(p);
p.x = 2.;
p.y = 0;
p.z = 0;
mine.points.push_back(p);
p.x = -3.;
p.y = 0;
p.z = 0;
mine.points.push_back(p);
p.x = -2.;
p.y = 0;
p.z = 0;
mine.points.push_back(p);
p.x = 0;
p.y = -3.;
p.z = 0;
mine.points.push_back(p);
p.x = 0;
p.y = 2.;
p.z = 0;
mine.points.push_back(p);
//publish path
mine_pub.publish(mine);
}
#endif
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices2;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_seg_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_RANSAC_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_whole_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr whole_pc (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr ball_candidate_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_plane_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_RANSAC_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_false_ball_candidate (new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> inliers;
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Pass through Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time pass_start = ros::Time::now();
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*passthrough_filtered);
passthrough_pub.publish(passthrough_filtered);
ros::Time pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for plane features pcl::SACSegmentation
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_seg_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*passthrough_filtered, *plane_seg_cloud);
// Optional
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setAxis(Eigen::Vector3f (0, -1, 0)); // best plane should be perpendicular to z-axis
seg.setMaxIterations (40);
seg.setDistanceThreshold (0.05);
seg.setInputCloud (plane_seg_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(plane_seg_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*plane_seg_output);
pcl::toROSMsg (*plane_seg_output, *plane_seg_output_cloud);
plane_pub.publish(plane_seg_output_cloud);
ros::Time plane_seg_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Extract rest plane and passthrough filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices.setNegative (true);
extract_indices.filter (*remove_plane_cloud);
plane_seg_cloud.swap (remove_plane_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*plane_seg_cloud, *rest_whole_cloud);
rest_whole_pub.publish(rest_whole_cloud); // 'rest_whole_cloud' substituted whole_pc
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time find_ball_start = ros::Time::now();
int iter = 0;
while(iter < 5)
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features pcl::SACSegmentation
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_whole_cloud, *sphere_cloud);
ros::Time sphere_start = ros::Time::now();
// Optional
seg.setOptimizeCoefficients (false);
// Mandatory
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.03);
seg.setRadiusLimits (0.12, 0.16);
seg.setInputCloud (sphere_cloud);
seg.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
if (inliers_sphere->indices.empty())
std::cerr << "[[--Can't find the Sphere Candidate.--]]" << std::endl;
else {
extract_indices.setInputCloud(sphere_cloud);
extract_indices.setIndices(inliers_sphere);
extract_indices.setNegative(false);
extract_indices.filter(*sphere_output);
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_seg_pub.publish(sphere_output_cloud); // 'sphere_output_cloud' means ball candidate point cloud
}
ros::Time sphere_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features pcl::SampleConsensusModelSphere
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_RANSAC_start = ros::Time::now();
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (sphere_output));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*sphere_output, inliers, *sphere_RANSAC_output);
pcl::toROSMsg (*sphere_RANSAC_output, *sphere_RANSAC_output_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* To discriminate a ball
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double w = 0;
w = double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
/double(sphere_output_cloud->width * sphere_output_cloud->height);
if (w > 0.9) {
BALL = true;
std::cout << "can find a ball" << std::endl;
true_ball_pub.publish(sphere_RANSAC_output_cloud);
break;
} else {
BALL = false;
std::cout << "can not find a ball" << std::endl;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Exclude false ball candidate
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices2.setInputCloud(plane_seg_cloud);
extract_indices2.setIndices(inliers_sphere);
extract_indices2.setNegative (true);
extract_indices2.filter (*remove_false_ball_candidate);
sphere_RANSAC_output.swap (remove_false_ball_candidate);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*sphere_RANSAC_output, *ball_candidate_output_cloud);
rest_ball_candidate_pub.publish(ball_candidate_output_cloud);
rest_whole_cloud = ball_candidate_output_cloud;
}
iter++;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_RANSAC_end = ros::Time::now();
}
ros::Time find_ball_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time whole_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << plane_seg_output_cloud->width * plane_seg_output_cloud->height << std::endl;
std::cout << "rest size : " << rest_whole_cloud->width * rest_whole_cloud->height << std::endl;
std::cout << "sphere size : " << sphere_output_cloud->width * sphere_output_cloud->height << std::endl;
std::cout << "sphere RANSAC size : " << sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height << " " << sphere_RANSAC_output->points.size() << std::endl;
std::cout << "sphereness : " << double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
/double(sphere_output_cloud->width * sphere_output_cloud->height) << std::endl;
std::cout << "model coefficient : " << coefficients_plane->values[0] << " "
<< coefficients_plane->values[1] << " "
<< coefficients_plane->values[2] << " "
<< coefficients_plane->values[3] << " " << std::endl;
printf("\n");
std::cout << "whole time : " << whole_end - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
std::cout << "passthrough time : " << pass_end - pass_start << " sec" << std::endl;
std::cout << "plane time : " << plane_seg_end - plane_seg_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "find a ball time : " << find_ball_end - find_ball_start << " sec" << std::endl;
//std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
//std::cout << "sphere ransac time : " << sphere_RANSAC_end - sphere_RANSAC_start << " sec" << std::endl;
printf("\n----------------------------------------------------------------------------\n");
printf("\n");
}
void lines_callback(const art_lrf::Lines::ConstPtr& msg_lines) {
if ((msg_lines->theta_index.size() != 0) &&
(msg_lines->est_rho.size() != 0) && (msg_lines->endpoints.size() !=
0)) {
scan2.reset(new vector<line> ());
line temp;
geometry_msgs::Point32 temp2;
geometry_msgs::Point32 temp_theta_index;
vector<int> temp3;
for (int i = 0; i < msg_lines->theta_index.size(); i++) {
temp.theta_index = msg_lines->theta_index[i];
temp.est_rho = msg_lines->est_rho[i];
for (int j = 0; j < msg_lines->endpoints[i].points.size(); j++) {
temp2 = msg_lines->endpoints[i].points[j];
temp3.push_back(temp2.x);
temp3.push_back(temp2.y);
temp.endpoints.push_back(temp3);
}
scan2->push_back(temp);
temp.endpoints.clear();
}
if (firstRun == true) {
firstRun = false;
scan1 = scan2;
previous_translation << 0,0;
}
else
{
compare_scans(scan1, scan2, previous_est_rot, est_rot,
previous_translation, est_translation );
if( pow(pow(est_translation[0],2) + pow(est_translation[1],2),0.5) >
WAYPOINT_THRESHOLD)
{
measurement new_base;
for(int i=0; i<scan1->size(); i++)
{
new_base.lines.push_back(scan1->at(i));
}
new_base.x = base_translation(0) + est_translation(0);
new_base.y = base_translation(1) + est_translation(1);
new_base.yaw = base_rot;
// float min_dist = 2*REMATCH_CANDIDATE_THRESH; //start with
// arbitrarily high value
// int min_index = 0;
// for( int i=0; i<base_scans.size(); i++)
// {
// float current_dist = base_scans[i].dist_from(new_base);
// if( current_dist < min_dist )
// {
// min_index = i;
// min_dist = current_dist;
// }
// }
// if(min_dist < REMATCH_CANDIDATE_THRESH)
// {
// boost::shared_ptr<vector<line> > temp_boost_scan(new vector<line>);
// for(int i=0; i<base_scans[min_index].lines.size(); i++)
// {
// temp_boost_scan->push_back(base_scans[min_index].lines[i]);
// }
// int expected_rot_difference = new_base.yaw -
//base_scans[min_index].yaw;
// int actual_rot_difference;
// Vector2f expected_translation_difference;
// expected_translation_difference << new_base.x -
//base_scans[min_index].x, new_base.y - base_scans[min_index].y;
// Vector2f actual_translation_difference;
// compare_scans(temp_boost_scan, scan2,
// new_base.yaw - base_scans[min_index].yaw,
//actual_rot_difference,
// expected_translation_difference,
//actual_translation_difference );
// float translation_error =
// pow(pow(expected_translation_difference(0) -
// actual_translation_difference(0), 2) +
// pow(expected_translation_difference(1) -
// actual_translation_difference(1),2), 0.5);
// if( (translation_error < REMATCH_VALIDATION_THRESH) &&
//(abs(expected_rot_difference - actual_rot_difference) < 7))
// {
// //overwrite the new_base values with the difference
//between the waypoint and the current scan
// new_base.x = base_scans[min_index].x +
//actual_translation_difference[0];
// new_base.y = base_scans[min_index].y +
//actual_translation_difference[1];
//; // new_base.yaw = base_scans[min_index].yaw + actual_rot_difference;
// }
// }
scan1 = scan2;
base_translation << new_base.x, new_base.y;
base_rot = new_base.yaw;
base_scans.push_back(new_base);
est_translation << 0, 0;
est_rot = 0;
}
previous_translation = est_translation;
previous_est_rot = est_rot;
}
geometry_msgs::Point32 pos_msg;
pos_msg.x = est_translation(0);
pos_msg.y = est_translation(1);
pos_msg.z = est_rot;
pub_pos.publish(pos_msg);
pos_msg_old.x = pos_msg.x;
pos_msg_old.y = pos_msg.y;
pos_msg_old.z = pos_msg.z;
}
else {
cout << endl << "MISSED SCAN" << endl;
pub_pos.publish(pos_msg_old);
}
}
void
TBK_Node::keyboardLoop()
{
char keyboard_input;
double max_tv = max_speed;
double max_rv = max_turn;
bool dirty=false;
int speed=0;
int turn=0;
// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
puts("Reading from keyboard");
puts("---------------------------");
puts("q/z : increase/decrease max angular and linear speeds by 10%");
puts("w/x : increase/decrease max linear speed by 10%");
puts("e/c : increase/decrease max angular speed by 10%");
puts("---------------------------");
puts("Moving around:");
puts(" u i o");
puts(" j k l");
puts(" m , .");
puts("1/2 kick left/right");
puts("3/4 get up from forward/backwards falll");
puts("arrow keys move head");
puts("---------------------------");
puts("6 stand up action");
puts("7 sit down action");
puts("9 start walking gait");
puts("0 stop walking gait" );
puts("---------------------------");
puts("anything else : stop");
puts("---------------------------");
struct pollfd ufd;
ufd.fd = kfd;
ufd.events = POLLIN;
for(;;)
{
boost::this_thread::interruption_point();
// get the next event from the keyboard
int num;
if((num = poll(&ufd, 1, 250)) < 0)
{
perror("poll():");
return;
}
else if(num > 0)
{
if(read(kfd, &keyboard_input, 1) < 0)
{
perror("read():");
return;
}
}
else
continue;
switch ( keyboard_input ) {
case KEYCODE_I:
ROS_INFO("i Go straight without rotation");
speed = 1;
turn = 0;
dirty = true;
break;
case KEYCODE_J:
ROS_INFO("j Turn left at a fixed position");
speed = 0;
turn = 1;
dirty = true;
break;
case KEYCODE_COMMA:
ROS_INFO(", Go backward without rotation");
speed = -1;
turn = 0;
dirty = true;
break;
case KEYCODE_O:
ROS_INFO("O Move forward with right turn");
speed = 1;
turn = -1;
dirty = true;
break;
case KEYCODE_L:
ROS_INFO("l Turn right at a fixed position");
speed = 0;
turn = -1;
dirty = true;
break;
case KEYCODE_U:
ROS_INFO("U Move forward with left turn");
speed = 1;
turn = 1;
dirty = true;
break;
case KEYCODE_M:
ROS_INFO("m Move backward with clockwise rotation");
speed = -1;
turn = -1;
dirty = true;
break;
case KEYCODE_PERIOD:
ROS_INFO(". Move backward with counter clockwise rotation");
speed = 1;
turn = -1;
dirty = true;
break;
case KEYCODE_K:
ROS_INFO(" k Stop");
speed = 0;
turn = 0;
dirty = true;
break;
case KEYCODE_Q:
ROS_INFO("q Increase angular and linear speeds by 10 percent");
max_tv += max_tv / 10;
max_rv += max_rv / 10;
if(always_command)
dirty=true;
break;
case KEYCODE_Z:
ROS_INFO("Z Decrease angular and linear speeds 10 percent");
max_tv -= max_tv / 10;
max_rv -= max_rv / 10;
if(always_command)
dirty=true;
break;
case KEYCODE_W:
ROS_INFO("w Increase linear speed by 10 percent");
max_tv += max_tv / 10;
if(always_command)
dirty=true;
break;
case KEYCODE_X:
ROS_INFO("X Decrease linear speed by 10 percent");
max_tv -= max_tv / 10;
if(always_command)
dirty=true;
break;
case KEYCODE_E:
ROS_INFO("e Increase angular speed by 10 percent");
max_rv += max_rv / 10;
if(always_command)
dirty=true;
break;
case KEYCODE_C:
ROS_INFO("C Decrease angular speed by 10 percent");
max_rv -= max_rv / 10;
if(always_command)
dirty=true;
break;
case KEYCODE_9:
ROS_INFO("9 enable walking");
enable_walking_msg.data = 1;
enable_walking_pub_.publish(enable_walking_msg);
break;
case KEYCODE_0:
ROS_INFO("0 disable walking");
enable_walking_msg.data = 0;
enable_walking_pub_.publish(enable_walking_msg);
break;
case KEYCODE_1:
ROS_INFO("1 kick left");
action_index_msg.data = 13;
action_pub_.publish(action_index_msg);
break;
case KEYCODE_2:
ROS_INFO("2 kick right");
action_index_msg.data = 12;
action_pub_.publish(action_index_msg);
break;
case KEYCODE_3:
ROS_INFO("3 get up from forward fall");
action_index_msg.data = 10;
action_pub_.publish(action_index_msg);
break;
case KEYCODE_4:
ROS_INFO("4 get up from backwards fall");
action_index_msg.data = 11;
action_pub_.publish(action_index_msg);
break;
case KEYCODE_6:
ROS_INFO("6 stand up action");
sit_stand_msg.data = 1;
sit_stand_pub_.publish(sit_stand_msg);
break;
case KEYCODE_7:
ROS_INFO("7 sit down action");
sit_stand_msg.data = 0;
sit_stand_pub_.publish(sit_stand_msg);
break;
case KEYCODE_UP:
ROS_INFO("head up");
if ( tilt < 0.24 ) angle_msg.data = tilt+0.05; //User determined limit
else angle_msg.data = tilt;
tilt_pub_.publish(angle_msg);
break;
case KEYCODE_DOWN:
ROS_INFO("head down");
if ( tilt > -0.35 ) angle_msg.data = tilt-0.05; //User determined limit
else angle_msg.data = tilt;
tilt_pub_.publish(angle_msg);
break;
case KEYCODE_RIGHT:
ROS_INFO("head right");
angle_msg.data = pan-0.05;
pan_pub_.publish(angle_msg);
break;
case KEYCODE_LEFT:
ROS_INFO("head left");
angle_msg.data = pan+0.05;
pan_pub_.publish(angle_msg);
break;
case KEYCODE_ARROW_1:
break;
case KEYCODE_ARROW_2:
break;
default:
ROS_INFO("default");
speed = 0;
turn = 0;
dirty = true;
}
// std::cout << std::hex;
// std::cout << (int)keyboard_input << std::endl;
if (dirty == true)
{
cmdvel.linear.x = speed * max_tv;
cmdvel.angular.z = turn * max_rv;
pub_.publish(cmdvel);
dirty = false;
}
}
}
///////////////////////////////////////////////////////////////////
///Syncronize objects only for clouds
//////////////////////////////////////////////////////////////////
void Callback(const sensor_msgs::PointCloud2ConstPtr& cloudI)
{
ROS_INFO("cloud timestamps %i.%i ", cloudI->header.stamp.sec,cloudI->header.stamp.nsec);
//////////////////////////////////////////////////////////////////
///for range images
/////////////////////////////////////////////////////////////////
Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::LASER_FRAME;
float noiseLevel=0.00;
float minRange = 0.0f;
int borderSize = 1;
/////////////////////////////////////////////////////////////////////
/// point cloud
///////////////////////////////////////////////////////////////////
PointCloud<PointXYZ>::Ptr cloud_ptr (new PointCloud<PointXYZ>);
if ((cloudI->width * cloudI->height) == 0)
return ;
ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
(int)cloudI->width * cloudI->height,
cloudI->header.frame_id.c_str (),
pcl::getFieldsList (*cloudI).c_str ());
pcl::fromROSMsg(*cloudI, *cloud_ptr);
///segment data only the part in front of
SegmentData(cloud_ptr,cloud_filtered,cloud_nonPlanes,cloud_Plane,cloud_projected);
///////////////////////////////////////////////
///image range images
//////////////////////////////////////////////
range_image->createFromPointCloud (*cloud_filtered, pcl::deg2rad(0.2f),
pcl::deg2rad (360.f), pcl::deg2rad (180.0f),
sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
/////////////////////////////////////////////////////////////////////////////////////////////
///planar range image TESTING
//////////////////////////////////////////////////////////////////////////////////////////
Eigen::Affine3f trans,P_affine;//does not include K
trans (0,0)= 0.0f; trans (0,1)= 0.0f; trans (0,2)=-1.0f; trans(0,3)=0.0f;
trans (1,0)= 0.0f; trans (1,1)= 1.0f; trans (1,2)=0.0f; trans (1,3)=0.0f;
trans (2,0)= 1.0f; trans (2,1)= 0.0f; trans (2,2)=0.0f; trans (2,3)=0.0f;
trans (3,0)= 0.0f; trans (3,1)= 0.0f; trans (3,2)=0.0f; trans (3,3)=1.0f;
P_affine (0,0)= 0.0187f; P_affine (0,1)= -0.8024f; P_affine (0,2)= -0.5965f; P_affine(0,3)=-0.7813f;
P_affine (1,0)= -0.9998f; P_affine (1,1)= -0.0168f; P_affine (1,2)=-0.0088f; P_affine (1,3)=0.1818f;
P_affine (2,0)= -0.0030f; P_affine (2,1)= 0.5965f; P_affine (2,2)=-0.8026f; P_affine (2,3)=0.7670f;
P_affine (3,0)= 0.0f; P_affine (3,1)= 0.0f; P_affine (3,2)=0.0f; P_affine (3,3)=1.0f;
// 0.0187 -0.8024 -0.5965 -0.7813
// -0.9998 -0.0168 -0.0088 0.1818
// -0.0030 0.5965 -0.8026 0.7670
for(unsigned int i=0; i<cloud_filtered->size();i++)
{
PointXYZ p1,p2;
p1=cloud_filtered->points[i];
p2.x=-p1.z;
p2.y=p1.y;
p2.z=p1.x;
cloud_trans->push_back(p2);
}
// pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, trans);
Eigen::Affine3f pose(Eigen::Affine3f::Identity());
// pose=pose*Translation3f(T_);
pose=P_affine;
///
range_image_planar->createFromPointCloudWithFixedSize(*cloud_filtered,768, 1024,
//K_(0,2),K_(1,2),K_(0,0),K_(1,1),P_affine
K_(0,2),K_(1,2),K_(0,0),K_(1,1), Eigen::Affine3f::Identity()
,pcl::RangeImage::LASER_FRAME, noiseLevel, minRange);
//GenerarImagenOpenCV(range_image,im_range_current, im_range_current_norm);
//TESTING PART with planar range image
GenerarImagenOpenCV(range_image_planar,im_range_current, im_range_current_norm);
applyColorMap( im_range_current_norm, falseColorsMap, COLORMAP_HOT);
if(!firstFrame)
{
Mat im2 = Mat(im_range_previous_norm.rows, im_range_previous_norm.cols, im_range_previous_norm.type());
cv::resize(im_range_current_norm, im2,im2.size(), INTER_LINEAR );
cv::calcOpticalFlowFarneback(im2,im_range_previous_norm,flow,0.5, 3, 15, 3, 5, 1.2, 0);
///show data
cv::cvtColor( im2 ,cflow, CV_GRAY2BGR);
drawOptFlowMap(flow, cflow, 10, 1.5, Scalar(0, 255, 0));
drawOpticalFlow_color(flow,color_flow, -1);
//Set_range_flow(range_image,im2,flow,cloud_flow,cloud_p3d,normals,cloud_rgb);
///planar range image TESTING
Set_range_flow(range_image_planar,im2,flow,cloud_flow,cloud_p3d,normals,cloud_rgb);
////////////////////////////////////////////////////////////////////////////////////
///publishing
//////////////////////////////////////////////////////////////////////////////////
cv_bridge::CvImagePtr cv_send(new cv_bridge::CvImage);
cv_send->image=Mat(falseColorsMap.rows, falseColorsMap.cols, CV_8UC3);
cv_send->encoding=enc::RGB8;
///range image
falseColorsMap.copyTo(cv_send->image);
pub_im_range.publish(cv_send->toImageMsg());
//flow arrows
cv_send->image=Mat(cflow.rows, cflow.cols, CV_8UC3);
cflow.copyTo(cv_send->image);
pub_im_range_flow.publish(cv_send->toImageMsg());
///color flow
cv_send->image=Mat( color_flow.rows, color_flow.cols, CV_8UC3);
color_flow.copyTo(cv_send->image);
pub_im_range_color_flow.publish(cv_send->toImageMsg());
///cloud flow
sensor_msgs::PointCloud2 output_cloud;
pcl::toROSMsg(*cloud_rgb,output_cloud);
output_cloud.header.frame_id = cloudI->header.frame_id;
pub_cloud_flow.publish(output_cloud);
////////////////////////////////////
///cloud complete
///////////////////////////////////////
pub_cloud.publish(cloudI);
////////////////////////////////////////////
///point normal
/////////////////////////////////////////////
sensor_msgs::PointCloud2 output_cloud2;
pcl::concatenateFields(*cloud_p3d,* normals,*cloud_normals);
pcl::toROSMsg(*cloud_normals,output_cloud2);
output_cloud2.header.frame_id = cloudI->header.frame_id;
pub_cloud_normal.publish(output_cloud2);
//////////////////////////////////////////////////////////////
///markers
///////////////////////////////////////////////////////////
visualization_msgs::Marker points,line_list;
Publish_Marks(cloud_flow,cloud_p3d, points,line_list);
pub_markers.publish(points);
pub_markers.publish(line_list);
}
//create the images
firstFrame=0;// we have read the first frame
im_range_current_norm.copyTo(im_range_previous_norm);
flow=Mat(im_range_current_norm.rows, im_range_current_norm.cols,CV_32FC2);
cflow=Mat(im_range_current_norm.rows, im_range_current_norm.cols, CV_8UC3);
}
// In this function, the Voronoi cell is calculated, integrated and the new goal point is calculated and published.
void SimpleDeployment::publish()
{
if ( got_map_ && got_me_ ) {
double factor = map_.info.resolution / resolution_; // zoom factor for openCV visualization
ROS_DEBUG("SimpleDeployment: Map received, determining Voronoi cells and publishing goal");
removeOldAgents();
// Create variables for x-values, y-values and the maximum and minimum of these, needed for VoronoiDiagramGenerator
float xvalues[agent_catalog_.size()];
float yvalues[agent_catalog_.size()];
double xmin = 0.0, xmax = 0.0, ymin = 0.0, ymax = 0.0;
cv::Point seedPt = cv::Point(1,1);
// Create empty image with the size of the map to draw points and voronoi diagram in
cv::Mat vorImg = cv::Mat::zeros(map_.info.height*factor,map_.info.width*factor,CV_8UC1);
for ( uint i = 0; i < agent_catalog_.size(); i++ ) {
geometry_msgs::Pose pose = agent_catalog_[i].getPose();
// Keep track of x and y values
xvalues[i] = pose.position.x;
yvalues[i] = pose.position.y;
// Keep track of min and max x
if ( pose.position.x < xmin ) {
xmin = pose.position.x;
} else if ( pose.position.x > xmax ) {
xmax = pose.position.x;
}
// Keep track of min and max y
if ( pose.position.y < ymin ) {
ymin = pose.position.y;
} else if ( pose.position.y > ymax ) {
ymax = pose.position.y;
}
// Store point as seed point if it represents this agent
if ( agent_catalog_[i].getName() == this_agent_.getName() ){
// Scale positions in metric system column and row numbers in image
int c = ( pose.position.x - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r = map_.info.height * factor - ( pose.position.y - map_.info.origin.position.y ) * factor / map_.info.resolution;
cv::Point pt = cv::Point(c,r);
seedPt = pt;
}
// Draw point on image
// cv::circle( vorImg, pt, 3, WHITE, -1, 8);
}
ROS_DEBUG("SimpleDeployment: creating a VDG object and generating Voronoi diagram");
// Construct a VoronoiDiagramGenerator (VoronoiDiagramGenerator.h)
VoronoiDiagramGenerator VDG;
xmin = map_.info.origin.position.x; xmax = map_.info.width * map_.info.resolution + map_.info.origin.position.x;
ymin = map_.info.origin.position.y; ymax = map_.info.height * map_.info.resolution + map_.info.origin.position.y;
// Generate the Voronoi diagram using the collected x and y values, the number of points, and the min and max x and y values
VDG.generateVoronoi(xvalues,yvalues,agent_catalog_.size(),float(xmin),float(xmax),float(ymin),float(ymax));
float x1,y1,x2,y2;
ROS_DEBUG("SimpleDeployment: collecting line segments from the VDG object");
// Collect the generated line segments from the VDG object
while(VDG.getNext(x1,y1,x2,y2))
{
// Scale the line segment end-point coordinates to column and row numbers in image
int c1 = ( x1 - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r1 = vorImg.rows - ( y1 - map_.info.origin.position.y ) * factor / map_.info.resolution;
int c2 = ( x2 - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r2 = vorImg.rows - ( y2 - map_.info.origin.position.y ) * factor / map_.info.resolution;
// Draw line segment
cv::Point pt1 = cv::Point(c1,r1),
pt2 = cv::Point(c2,r2);
cv::line(vorImg,pt1,pt2,WHITE);
}
ROS_DEBUG("SimpleDeployment: drawing map occupancygrid and resizing to voronoi image size");
// Create cv image from map data and resize it to the same size as voronoi image
cv::Mat mapImg = drawMap();
cv::Mat viewImg(vorImg.size(),CV_8UC1);
cv::resize(mapImg, viewImg, vorImg.size(), 0.0, 0.0, cv::INTER_NEAREST );
// Add images together to make the total image
cv::Mat totalImg(vorImg.size(),CV_8UC1);
cv::bitwise_or(viewImg,vorImg,totalImg);
cv::Mat celImg = cv::Mat::zeros(vorImg.rows+2, vorImg.cols+2, CV_8UC1);
cv::Scalar newVal = cv::Scalar(1), upDiff = cv::Scalar(100), loDiff = cv::Scalar(256);
cv::Rect rect;
cv::floodFill(totalImg,celImg,seedPt,newVal,&rect,loDiff,upDiff,4 + (255 << 8) + cv::FLOODFILL_MASK_ONLY);
// Compute the center of mass of the Voronoi cell
cv::Moments m = moments(celImg, false);
cv::Point centroid(m.m10/m.m00, m.m01/m.m00);
cv::circle( celImg, centroid, 3, BLACK, -1, 8);
// Convert seed point to celImg coordinate system (totalImg(x,y) = celImg(x+1,y+1)
cv::Point onePt(1,1);
centroid = centroid - onePt;
for ( uint i = 0; i < agent_catalog_.size(); i++ ){
int c = ( xvalues[i] - map_.info.origin.position.x ) * factor / map_.info.resolution;
int r = map_.info.height * factor - ( yvalues[i] - map_.info.origin.position.y ) * factor / map_.info.resolution;
cv::Point pt = cv::Point(c,r);
cv::circle( totalImg, pt, 3, GRAY, -1, 8);
}
cv::circle( totalImg, seedPt, 3, WHITE, -1, 8);
cv::circle( totalImg, centroid, 2, WHITE, -1, 8); //where centroid is the goal position
// Due to bandwidth issues, only display this image if requested
if (show_cells_) {
cv::imshow( "Voronoi Cells", totalImg );
}
cv::waitKey(3);
// Scale goal position in map back to true goal position
geometry_msgs::Pose goalPose;
// goalPose.position.x = centroid.x * map_.info.resolution / factor + map_.info.origin.position.x;
goalPose.position.x = centroid.x / factor + map_.info.origin.position.x;
// goalPose.position.y = (map_.info.height - centroid.y / factor) * map_.info.resolution + map_.info.origin.position.y;
goalPose.position.y = (map_.info.height - centroid.y / factor) + map_.info.origin.position.y;
double phi = atan2( seedPt.y - centroid.y, centroid.x - seedPt.x );
goalPose.orientation = tf::createQuaternionMsgFromYaw(phi);
goal_.pose = goalPose;
goal_.header.frame_id = "map";
goal_.header.stamp = ros::Time::now();
goal_pub_.publish(goal_);
// move_base_msgs::MoveBaseGoal goal;
// goal.target_pose.header.frame_id = "map";
// goal.target_pose.header.stamp = ros::Time::now();
// goal.target_pose.pose = goalPose;
// ac_.sendGoal(goal);
}
else {
ROS_DEBUG("SimpleDeployment: No map received");
}
}
void matchThreshCb( int val, void *dummy )
{
std_msgs::Float32 msg;
msg.data = val / 100.0;
matchThreshPub.publish( msg );
}
void image_callback(const sensor_msgs::LaserScan::ConstPtr& laser,
const sensor_msgs::CompressedImage::ConstPtr& image) {
if (!this->bot.valid()) {
return;
}
try {
// Convert from ROS image msg to OpenCV matrix images
cv_bridge::CvImagePtr cv_ptr;
// cv_ptr = cv_bridge::toCvCopy((sensor_msgs::Imageu) image, sensor_msgs::image_encodings::BGR8);
// cv::Mat src = cv_ptr->image;
cv::Mat src = cv::imdecode(image->data, 1);
// Take hsv ranges from launch
// OpenCV filters to find colours
cv::Mat hsv, pink_threshold, yellow_threshold, blue_threshold, green_threshold;
cv::cvtColor(src, hsv, CV_BGR2HSV);
cv::inRange(hsv, cv::Scalar(pink_ranges[0],pink_ranges[2],pink_ranges[3]), cv::Scalar(pink_ranges[1],255,255), pink_threshold);
cv::inRange(hsv, cv::Scalar(yellow_ranges[0],yellow_ranges[2],yellow_ranges[3]), cv::Scalar(yellow_ranges[1],255,255), yellow_threshold);
cv::inRange(hsv, cv::Scalar(blue_ranges[0],blue_ranges[2],blue_ranges[3]), cv::Scalar(blue_ranges[1],255,255), blue_threshold);
cv::inRange(hsv, cv::Scalar(green_ranges[0],green_ranges[2],green_ranges[3]), cv::Scalar(green_ranges[1],255,255), green_threshold);
blue_threshold = cv::Scalar::all(255) - blue_threshold;
pink_threshold = cv::Scalar::all(255) - pink_threshold;
yellow_threshold = cv::Scalar::all(255) - yellow_threshold;
green_threshold = cv::Scalar::all(255) - green_threshold;
// blob detection
cv::SimpleBlobDetector::Params params;
params.filterByArea = 1;
params.minArea = 200;
params.maxArea = 100000;
params.filterByConvexity = 1;
params.minConvexity = 0.5;
params.filterByInertia = true;
params.minInertiaRatio = 0.65;
// find keypoints
std::vector<cv::KeyPoint> pink_keypoints, yellow_keypoints, blue_keypoints, green_keypoints;
cv::SimpleBlobDetector detector(params);
detector.detect(pink_threshold, pink_keypoints);
detector.detect(yellow_threshold, yellow_keypoints);
detector.detect(blue_threshold, blue_keypoints);
detector.detect(green_threshold, green_keypoints);
cv::Mat blobs;
cv::drawKeypoints (src, pink_keypoints, blobs,
cv::Scalar(0,0,255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv::drawKeypoints (blobs, blue_keypoints, blobs,
cv::Scalar(255,0,0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv::drawKeypoints (blobs, green_keypoints, blobs,
cv::Scalar(0,255,0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv::drawKeypoints (blobs, yellow_keypoints, blobs,
cv::Scalar(125,125,125), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
// acceptable horizontal distance between the 2 colours on a pillar
for (auto pink_pt = pink_keypoints.begin(); pink_pt != pink_keypoints.end(); pink_pt++) {
ROS_INFO_STREAM("Pink x= " << pink_pt->pt.x);
double xCo = pink_pt->pt.x;
xCo = 320 - xCo;
// TODO: you might want atan2
double theta = atan2(xCo*tan(29 * M_PI / 180), 320.0);
//ROS_INFO_STREAM("theta: " << (theta * 180 / M_PI));
double lTheta = theta - laser->angle_min;
int distance_index = lTheta / laser->angle_increment;
/*
double r2 = laser->ranges[distance_index];
double r1 = 0.1;
*/
// double distance = sqrt( (r1 + r2*cos(theta))*(r1 + r2*cos(theta)) + (r2*sin(theta))*(r2*sin(theta)) );
double distance = laser->ranges[distance_index];
ROS_INFO_STREAM("BEACON IS " << distance << " FROM BOT");
if( std::isfinite(distance) && std::isfinite(theta) && distance < 2.0 ) {
distance = distance - 0.3;
//ROS_INFO_STREAM("theta " << (theta * 180 / M_PI) << " distance " << distance);
search_for_match(blue_keypoints.begin(), blue_keypoints.end(), pink_pt, "blue", make_pair(distance, theta));
search_for_match(yellow_keypoints.begin(), yellow_keypoints.end(), pink_pt, "yellow", make_pair(distance, theta));
search_for_match(green_keypoints.begin(), green_keypoints.end(), pink_pt, "green", make_pair(distance, theta));
}
}
// gui display
imshow("blobs", blobs);
bool found_all = true;
int count = 0;
for (auto it = beacons.begin(); it != beacons.end(); ++it) {
if (!it->found()) {
found_all = false;
} else {
ROS_INFO_STREAM("Found T: " << it->top << " B: " << it->bottom << " at " << it->x << " " << it->y);
count++;
}
}
ROS_INFO_STREAM("Found " << count << " of " << beacons.size() << " beacons.");
if (found_all && this->bot.valid()) {
ROS_INFO("Found all beacons");
// Send beacon message
ass1::FoundBeacons msg;
msg.n = beacons.size();
msg.positions.clear();
for (auto it = beacons.begin(); it != beacons.end(); ++it) {
geometry_msgs::Point point;
point.x = it->x;
point.y = it->y;
point.z = 0;
msg.positions.push_back(point);
}
beacons_pub.publish(msg);
//shutdown subscriptions
img_sub.unsubscribe();
laser_sub.unsubscribe();
odom_sub.shutdown();
}
cv::waitKey(30);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("Could not convert from to 'bgr8'.");
}
}
void stopRobot()
{
cmdvel.linear.x = cmdvel.angular.z = 0.0;
pub_.publish(cmdvel);
}
// New depth sample event varsace tieshandler
void onNewDepthSample(DepthNode node, DepthNode::NewSampleReceivedData data)
{
//printf("Z#%u: %d %d %d\n",g_dFrames,data.vertices[500].x,data.vertices[500].y,data.vertices[500].z);
int count = 0;
msg->header.frame_id = "/base_link";
// Project some 3D points in the Color Frame
if (!g_pProjHelper)
{
g_pProjHelper = new ProjectionHelper (data.stereoCameraParameters);
g_scp = data.stereoCameraParameters;
}
else if (g_scp != data.stereoCameraParameters)
{
g_pProjHelper->setStereoCameraParameters(data.stereoCameraParameters);
g_scp = data.stereoCameraParameters;
}
int32_t w, h;
FrameFormat_toResolution(data.captureConfiguration.frameFormat,&w,&h);
int cx = w/2;
int cy = h/2;
msg->points.resize(w*h);
Vertex p3DPoints[4];
p3DPoints[0] = data.vertices[(cy-h/4)*w+cx-w/4];
p3DPoints[1] = data.vertices[(cy-h/4)*w+cx+w/4];
p3DPoints[2] = data.vertices[(cy+h/4)*w+cx+w/4];
p3DPoints[3] = data.vertices[(cy+h/4)*w+cx-w/4];
Point2D p2DPoints[4];
g_pProjHelper->get2DCoordinates ( p3DPoints, p2DPoints, 4, CAMERA_PLANE_COLOR);
g_dFrames++;
msg->height = h;
cloud.height = h;
msg->width = w;
cloud.width = w;
cloud.is_dense = true;
cloud.points.resize(w*h);
for(int i = 0; i < h; i++) {
for(int j = 0; j < w; j++) {
count++;
msg->points.push_back (pcl::PointXYZ(data.vertices[count].x,data.vertices[count].y,data.vertices[count].z));
cloud.points[count].x = data.vertices[count].x;
cloud.points[count].y = data.vertices[count].y;
if(data.vertices[count].z == 32001) {
cloud.points[count].z = 0;
} else {
cloud.points[count].z = data.vertices[count].z;
}
//cloud.points[count].rgb = 0;
}
}
// We now want to create a range image from the above point cloud, with a 1deg angular resolution
float angularResolution = (float) ( 1.0f * (M_PI/180.0f)); // 1.0 degree in radians
float maxAngleWidth = (float) (360.0f * (M_PI/180.0f)); // 360.0 degree in radians
float maxAngleHeight = (float) (180.0f * (M_PI/180.0f)); // 180.0 degree in radians
Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
float noiseLevel=0.00;
float minRange = 0.0f;
int borderSize = 0;
//pcl::RangeImage rangeImage;
//rangeImage.createFromPointCloud(cloud, angularResolution, maxAngleWidth, maxAngleHeight, sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
pub.publish (msg);
// Quit the main loop after 200 depth frames received
if (g_dFrames%1== 0) {
pcl::io::savePCDFileASCII("pcdTest.pcd", cloud);
g_context.quit();
}
}
void TeleopUAVController::keyboardLoop(){
char c;
bool dirty=false;
// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
intro_ = true;
puts("Reading from keyboard");
puts("---------------------------");
puts("***************************");
puts("Press Characters Once");
puts("***************************");
puts("");
puts("Use 'WASD' to translate");
puts("Use 'T' to take off");
puts("Use 'L' to land");
puts("Use 'H' to hover");
puts("Use 'QE' to yaw");
puts("Press 'Shift' to run");
puts("");
puts("---------------------------");
for(;;)
{
// get the next event from the keyboard
if(read(kfd, &c, 1) < 0)
{
perror("read():");
exit(-1);
}
cmd.linear.z = cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
switch(c)
{
// Walking
case KEYCODE_T:
cmd.linear.z = walk_vel;
ROS_INFO_STREAM("TAKE OFF!!");
dirty = true;
break;
case KEYCODE_L:
cmd.linear.z = - walk_vel;
ROS_INFO_STREAM("LAND!!");
dirty = true;
break;
case KEYCODE_H:
ROS_INFO_STREAM("** HOVERING **");
dirty = true;
break;
case KEYCODE_W:
cmd.linear.x = walk_vel;
ROS_INFO_STREAM("FORWARD");
dirty = true;
break;
case KEYCODE_S:
cmd.linear.x = - walk_vel;
ROS_INFO_STREAM("BACKWARD");
dirty = true;
break;
case KEYCODE_A:
cmd.linear.y = walk_vel;
ROS_INFO_STREAM("LEFT <-");
dirty = true;
break;
case KEYCODE_D:
cmd.linear.y = - walk_vel;
ROS_INFO_STREAM("RIGHT ->");
dirty = true;
break;
case KEYCODE_Q:
cmd.angular.z = yaw_rate;
dirty = true;
break;
case KEYCODE_E:
cmd.angular.z = - yaw_rate;
dirty = true;
break;
// Running
case KEYCODE_W_CAP:
cmd.linear.x = run_vel;
dirty = true;
break;
case KEYCODE_S_CAP:
cmd.linear.x = - run_vel;
dirty = true;
break;
case KEYCODE_A_CAP:
cmd.linear.y = run_vel;
dirty = true;
break;
case KEYCODE_D_CAP:
cmd.linear.y = - run_vel;
dirty = true;
break;
case KEYCODE_Q_CAP:
cmd.angular.z = yaw_rate_run;
dirty = true;
break;
case KEYCODE_E_CAP:
cmd.angular.z = - yaw_rate_run;
dirty = true;
break;
}
if (dirty == true)
{
vel_pub_.publish(cmd);
}
}
}
void
align_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
{
if (picked)
{
cloudrgbptr cloud_segment (new cloudrgb()); //Segmented point cloud from msg
pcl::fromROSMsg(*cloud, *cloud_segment); //Convert msg to point cloud
cloudrgbptr cloud_translated (new cloudrgb()); //Translated cloud using point
cloudrgbptr cloud_aligned (new cloudrgb()); //Aligned output from ICP
sensor_msgs::PointCloud2 cloud_aligned_msg; //create msg to publish
// Translate
Eigen::Matrix4f Tm;
Tm << 1, 0, 0, picked_x,
0, 1, 0, picked_y,
0, 0, 1, picked_z,
0, 0, 0, 1;
pcl::transformPointCloud(*cloud_source, *cloud_translated, Tm);
// ICP
// Setup ICP
pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
icp.setEuclideanFitnessEpsilon (1e-10);
icp.setTransformationEpsilon (1e-6);
icp.setMaxCorrespondenceDistance (.1);
icp.setMaximumIterations(50);
icp.setInputSource(cloud_translated);
icp.setInputTarget(cloud_segment);
//copy the source cloud
cloud_aligned = cloud_translated;
Eigen::Matrix4f prev;
//Forced iteractions with ICP loop
for (int i = 0; i < 10; ++i)
{
PCL_INFO ("Iteration Nr. %d.\n", i);
cloud_translated = cloud_aligned; //when you set to pointers equal does it actaully set the point clouds equal?
icp.setInputSource (cloud_translated);
icp.align (*cloud_aligned);
Tm = icp.getFinalTransformation () * Tm;
std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
//added change to each iteration
if (fabs ((icp.getLastIncrementalTransformation () - prev).sum ()) < icp.getTransformationEpsilon ())
icp.setMaxCorrespondenceDistance (icp.getMaxCorrespondenceDistance () - 0.001);
prev = icp.getLastIncrementalTransformation ();
}
std::cout << "Final Tm:" << std::endl;
std::cout << Tm << std::endl;
//Convert the pcl cloud back to rosmsg
pcl::toROSMsg(*cloud_aligned, cloud_aligned_msg);
//Set the header of the cloud
cloud_aligned_msg.header.frame_id = cloud->header.frame_id;
// Publish the data
//You may have to set the header frame id of the cloud_filtered also
pub1.publish (cloud_aligned_msg);
// Create the transform matrix between the origin and the center of the eye (when imported)
// NOTE: when imported the origin is at the top of the lifting eye
// This is to help when the lifting eye is translated to the picked point (which will probably
// be at the top of the lifting eye.
Eigen::Matrix4f centerOffset;
centerOffset << 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0.027,
0, 0, 0, 1;
// copy the original transform in case it is needed to transform the cloud later.
Eigen::Matrix4f originalTm = Tm;
// calculate center of lifting eye tranform
Tm = centerOffset * Tm;
tf::Vector3 origin;
origin.setValue(static_cast<double>(Tm(0,3)),static_cast<double>(Tm(1,3)),static_cast<double>(Tm(2,3)));
tf::Matrix3x3 tf3d;
tf3d.setValue( static_cast<double>(Tm(0,0)), static_cast<double>(Tm(0,1)), static_cast<double>(Tm(0,2)),
static_cast<double>(Tm(1,0)), static_cast<double>(Tm(1,1)), static_cast<double>(Tm(1,2)),
static_cast<double>(Tm(2,0)), static_cast<double>(Tm(2,1)), static_cast<double>(Tm(2,2)));
tf::Quaternion TmQt;
tf3d.getRotation(TmQt);
tf::Transform transform;
transform.setOrigin(origin);
transform.setRotation(TmQt);
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_rgb_optical_frame", "lifting_eye"));
// prep for marker
tf::Quaternion markerQt;
markerQt =tf::createQuaternionFromRPY(0,0,M_PI/2);
markerQt = markerQt * TmQt;
tf::Transform markerTf;
markerTf.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
markerTf.setRotation( markerQt );
// Marker
visualization_msgs::Marker marker;
marker.header.frame_id = "camera_rgb_optical_frame";
marker.header.stamp = ros::Time();
marker.ns = "my_namespace";
marker.id = 0;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = Tm(0,3);
marker.pose.position.y = Tm(1,3);
marker.pose.position.z = Tm(2,3);
marker.pose.orientation.x = markerQt.getY();
marker.pose.orientation.y = markerQt.getZ();
marker.pose.orientation.z = markerQt.getX();
marker.pose.orientation.w = markerQt.getW();
marker.scale.x = 0.005;
marker.scale.y = 0.01;
marker.scale.z = 0.2;
marker.color.a = 0.5;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
//vis_pub.publish( marker );
}
}
// Proccess the point clouds, called when subscriber gets msg
void cloudCallback( const sensor_msgs::PointCloud2ConstPtr& msg )
{
ROS_INFO_STREAM("Recieved callback");
// Basic point cloud conversions ---------------------------------------------------------------
// Convert from ROS to PCL
pcl::PointCloud<pcl::PointXYZRGB> cloud;
pcl::fromROSMsg(*msg, cloud);
// Make new point cloud that is in our working frame
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
// Transform to whatever frame we're working in, probably the arm's base frame, ie "base_link"
tf_listener_.waitForTransform(std::string(arm_link), cloud.header.frame_id,
cloud.header.stamp, ros::Duration(1.0));
if(!pcl_ros::transformPointCloud(std::string(arm_link), cloud, *cloud_transformed, tf_listener_))
{
ROS_ERROR("Error converting to desired frame");
return;
}
// Limit to things we think are roughly at the table height ------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filteredZ(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud(cloud_transformed);
pass.setFilterFieldName("z");
pass.setFilterLimits(min_cam_dist-.05,max_cam_dist+0.05);
pass.filter(*cloud_filteredZ);
// Limit to things in front of the robot ---------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pass.setInputCloud(cloud_filteredZ);
pass.setFilterFieldName("x");
pass.setFilterLimits(.1,.5);
pass.filter(*cloud_filtered);
// Check if any points remain
if( cloud_filtered->points.size() == 0 )
{
ROS_ERROR("0 points left");
return;
}
else
{
ROS_INFO("[block detection] Filtered, %d points left", (int) cloud_filtered->points.size());
}
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
int nr_points = cloud_filtered->points.size();
// Segment cloud until there are less than 30% of points left? not sure why this is necessary
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC); // robustness estimator - RANSAC is simple
seg.setMaxIterations(200);
seg.setDistanceThreshold(0.005); // determines how close a point must be to the model in order to be considered an inlier
while(cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud (find the table)
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if(inliers->indices.size() == 0)
{
ROS_ERROR("[block detection] Could not estimate a planar model for the given dataset.");
return;
}
// Debug output - DTC
// Show the contents of the inlier set, together with the estimated plane parameters, in ax+by+cz+d=0 form (general equation of a plane)
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
}//end while
// DTC: Removed to make compatible with PCL 1.5
// Creating the KdTree object for the search method of the extraction
//pcl::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
//tree->setInputCloud(cloud_filtered);
// Publish point cloud data
depth_pub_.publish(cloud_filtered);
}//end cloudCallBack
void green_light(bool status)
{
std_msgs::Bool msg;
msg.data = status;
_greenlight.publish(msg);
}
void speak(string val)
{
std_msgs::String msg;
msg.data = val;
_speak.publish(msg);
}
void red_light(bool status)
{
std_msgs::Bool msg;
msg.data = status;
_redlight.publish(msg);
}
void Tag_follow_class::publishtwist()
{
if(joy_automode)
vel_pub.publish(twist);
cout << "the parameters of the pid are " << vel_x_kp << " and " << vel_x_kd<< " " <<vel_y_kp << " " << vel_y_kd << endl;
}
void
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
last_laser_received_ts_ = ros::Time::now();
if( map_ == NULL ) {
return;
}
boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
int laser_index = -1;
// Do we have the base->base_laser Tx yet?
if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
{
ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
lasers_.push_back(new AMCLLaser(*laser_));
lasers_update_.push_back(true);
laser_index = frame_to_laser_.size();
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)),
ros::Time(), laser_scan->header.frame_id);
tf::Stamped<tf::Pose> laser_pose;
try
{
this->tf_->transformPose(base_frame_id_, ident, laser_pose);
}
catch(tf::TransformException& e)
{
ROS_ERROR("Couldn't transform from %s to %s, "
"even though the message notifier is in use",
laser_scan->header.frame_id.c_str(),
base_frame_id_.c_str());
return;
}
pf_vector_t laser_pose_v;
laser_pose_v.v[0] = laser_pose.getOrigin().x();
laser_pose_v.v[1] = laser_pose.getOrigin().y();
// laser mounting angle gets computed later -> set to 0 here!
laser_pose_v.v[2] = 0;
lasers_[laser_index]->SetLaserPose(laser_pose_v);
ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
laser_pose_v.v[0],
laser_pose_v.v[1],
laser_pose_v.v[2]);
frame_to_laser_[laser_scan->header.frame_id] = laser_index;
} else {
// we have the laser pose, retrieve laser index
laser_index = frame_to_laser_[laser_scan->header.frame_id];
}
// Where was the robot when this scan was taken?
pf_vector_t pose;
if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
laser_scan->header.stamp, base_frame_id_))
{
ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
return;
}
pf_vector_t delta = pf_vector_zero();
if(pf_init_)
{
// Compute change in pose
//delta = pf_vector_coord_sub(pose, pf_odom_pose_);
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
// See if we should update the filter
bool update = fabs(delta.v[0]) > d_thresh_ ||
fabs(delta.v[1]) > d_thresh_ ||
fabs(delta.v[2]) > a_thresh_;
update = update || m_force_update;
m_force_update=false;
// Set the laser update flags
if(update)
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
}
bool force_publication = false;
if(!pf_init_)
{
// Pose at last filter update
pf_odom_pose_ = pose;
// Filter is now initialized
pf_init_ = true;
// Should update sensor data
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
force_publication = true;
resample_count_ = 0;
}
// If the robot has moved, update the filter
else if(pf_init_ && lasers_update_[laser_index])
{
//printf("pose\n");
//pf_vector_fprintf(pose, stdout, "%.3f");
AMCLOdomData odata;
odata.pose = pose;
// HACK
// Modify the delta in the action data so the filter gets
// updated correctly
odata.delta = delta;
// Use the action data to update the filter
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
// Pose at last filter update
//this->pf_odom_pose = pose;
}
bool resampled = false;
// If the robot has moved, update the filter
if(lasers_update_[laser_index])
{
AMCLLaserData ldata;
ldata.sensor = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size();
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
//
// Construct min and max angles of laser, in the base_link frame.
tf::Quaternion q;
q.setRPY(0.0, 0.0, laser_scan->angle_min);
tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
try
{
tf_->transformQuaternion(base_frame_id_, min_q, min_q);
tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return;
}
double angle_min = tf::getYaw(min_q);
double angle_increment = tf::getYaw(inc_q) - angle_min;
// wrapping angle to [-pi .. pi]
angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
// Apply range min/max thresholds, if the user supplied them
if(laser_max_range_ > 0.0)
ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
else
ldata.range_max = laser_scan->range_max;
double range_min;
if(laser_min_range_ > 0.0)
range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
else
range_min = laser_scan->range_min;
// The AMCLLaserData destructor will free this memory
ldata.ranges = new double[ldata.range_count][2];
ROS_ASSERT(ldata.ranges);
for(int i=0;i<ldata.range_count;i++)
{
// amcl doesn't (yet) have a concept of min range. So we'll map short
// readings to max range.
if(laser_scan->ranges[i] <= range_min)
ldata.ranges[i][0] = ldata.range_max;
else
ldata.ranges[i][0] = laser_scan->ranges[i];
// Compute bearing
ldata.ranges[i][1] = angle_min +
(i * angle_increment);
}
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
// Resample the particles
if(!(++resample_count_ % resample_interval_))
{
pf_update_resample(pf_);
resampled = true;
}
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);
// Publish the resulting cloud
// TODO: set maximum rate for publishing
if (!m_force_update) {
geometry_msgs::PoseArray cloud_msg;
cloud_msg.header.stamp = ros::Time::now();
cloud_msg.header.frame_id = global_frame_id_;
cloud_msg.poses.resize(set->sample_count);
for(int i=0;i<set->sample_count;i++)
{
tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
tf::Vector3(set->samples[i].pose.v[0],
set->samples[i].pose.v[1], 0)),
cloud_msg.poses[i]);
}
particlecloud_pub_.publish(cloud_msg);
}
}
if(resampled || force_publication)
{
// Read out the current hypotheses
double max_weight = 0.0;
int max_weight_hyp = -1;
std::vector<amcl_hyp_t> hyps;
hyps.resize(pf_->sets[pf_->current_set].cluster_count);
for(int hyp_count = 0;
hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
{
double weight;
pf_vector_t pose_mean;
pf_matrix_t pose_cov;
if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
{
ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
break;
}
hyps[hyp_count].weight = weight;
hyps[hyp_count].pf_pose_mean = pose_mean;
hyps[hyp_count].pf_pose_cov = pose_cov;
if(hyps[hyp_count].weight > max_weight)
{
max_weight = hyps[hyp_count].weight;
max_weight_hyp = hyp_count;
}
}
if(max_weight > 0.0)
{
ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
/*
puts("");
pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
puts("");
*/
geometry_msgs::PoseWithCovarianceStamped p;
// Fill in the header
p.header.frame_id = global_frame_id_;
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose
p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
p.pose.pose.orientation);
// Copy in the covariance, converting from 3-D to 6-D
pf_sample_set_t* set = pf_->sets + pf_->current_set;
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
p.pose.covariance[6*i+j] = set->cov.m[i][j];
}
}
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
p.pose.covariance[6*5+5] = set->cov.m[2][2];
/*
printf("cov:\n");
for(int i=0; i<6; i++)
{
for(int j=0; j<6; j++)
printf("%6.3f ", p.covariance[6*i+j]);
puts("");
}
*/
pose_pub_.publish(p);
last_published_pose = p;
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}
latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
latest_tf_valid_ = true;
if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
}
else
{
ROS_ERROR("No pose!");
}
}
else if(latest_tf_valid_)
{
if (tf_broadcast_ == true)
{
// Nothing changed, so we'll just republish the last transform, to keep
// everybody happy.
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
}
// Is it time to save our last pose to the param server
ros::Time now = ros::Time::now();
if((save_pose_period.toSec() > 0.0) &&
(now - save_pose_last_time) >= save_pose_period)
{
this->savePoseToServer();
save_pose_last_time = now;
}
}
}
void
update (std::map<std::string, tf::StampedTransform>& frame_transforms)
{
bool transformation_healthy = true;
tf::StampedTransform transform;
try
{
listener->lookupTransform (BASE_FRAME.c_str (), TORSO_FRAME.c_str (), ros::Time (0), transform);
frame_transforms[TORSO_FRAME] = transform;
// std::cout << transform.getOrigin ().x () << " " << transform.getOrigin ().y () << " "
// << transform.getOrigin ().z () << std::endl;
// healthy_transformation[frame_names[i]] = true;
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
// healthy_transformation[frame_names[i]] = false;
transformation_healthy = false;
}
for (uint i = 1; i < frame_names.size (); i++)
{
try
{
listener->lookupTransform (TORSO_FRAME.c_str (), frame_names[i].c_str (), ros::Time (0), transform);
frame_transforms[frame_names[i]] = transform;
// std::cout << transform.getOrigin ().x () << " " << transform.getOrigin ().y () << " "
// << transform.getOrigin ().z () << std::endl;
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
transformation_healthy = false;
}
}
if (transformation_healthy)
{
// std::cout<<"****************"<<std::endl;
for (uint i = 0; i < frame_names.size (); i++)
{
if (!frame_names[i].compare (TORSO_FRAME))
{
gazebo_msgs::ModelState msg_model_st;
msg_model_st.model_name = "human";
msg_model_st.pose.position.x = frame_transforms[TORSO_FRAME].getOrigin ().x ();
msg_model_st.pose.position.y = frame_transforms[TORSO_FRAME].getOrigin ().y ();
msg_model_st.pose.position.z = frame_transforms[TORSO_FRAME].getOrigin ().z ();
msg_model_st.pose.orientation.x = frame_transforms[TORSO_FRAME].getRotation ().x ();
msg_model_st.pose.orientation.y = frame_transforms[TORSO_FRAME].getRotation ().y ();
msg_model_st.pose.orientation.z = frame_transforms[TORSO_FRAME].getRotation ().z ();
msg_model_st.pose.orientation.w = frame_transforms[TORSO_FRAME].getRotation ().w ();
msg_model_st.twist.angular.x = 0;
msg_model_st.twist.angular.y = 0;
msg_model_st.twist.angular.z = 0;
msg_model_st.twist.linear.x = 0;
msg_model_st.twist.linear.y = 0;
msg_model_st.twist.linear.z = 0;
msg_model_st.reference_frame = "world";
if (pub_model_st.getNumSubscribers ())
pub_model_st.publish (msg_model_st);
ros::spinOnce ();
gazebo_msgs::LinkState msg_link_st;
msg_link_st.link_name = HUMAN_NAMESPACE + TORSO_FRAME;
msg_link_st.pose = msg_model_st.pose;
msg_link_st.twist = msg_model_st.twist;
msg_link_st.reference_frame = "world";
if (pub_link_st.getNumSubscribers ())
pub_link_st.publish (msg_link_st);
}
for (uint i = 1; i < frame_names.size (); i++)
{
gazebo_msgs::LinkState msg_link_st;
msg_link_st.link_name = HUMAN_NAMESPACE + frame_names[i];
// std::cout << msg_link_st.link_name << std::endl;
msg_link_st.pose.position.x = frame_transforms[frame_names[i]].getOrigin ().x ();
msg_link_st.pose.position.y = frame_transforms[frame_names[i]].getOrigin ().y ();
msg_link_st.pose.position.z = frame_transforms[frame_names[i]].getOrigin ().z ();
msg_link_st.pose.orientation.x = frame_transforms[frame_names[i]].getRotation ().x ();
msg_link_st.pose.orientation.y = frame_transforms[frame_names[i]].getRotation ().y ();
msg_link_st.pose.orientation.z = frame_transforms[frame_names[i]].getRotation ().z ();
msg_link_st.pose.orientation.w = frame_transforms[frame_names[i]].getRotation ().w ();
msg_link_st.twist.angular.x = 0;
msg_link_st.twist.angular.y = 0;
msg_link_st.twist.angular.z = 0;
msg_link_st.twist.linear.x = 0;
msg_link_st.twist.linear.y = 0;
msg_link_st.twist.linear.z = 0;
msg_link_st.reference_frame = "human::torso_1";
// std::cout << msg_link_st.reference_frame << std::endl;
if (pub_link_st.getNumSubscribers ())
{
pub_link_st.publish (msg_link_st);
ros::spinOnce ();
}
}
}
// std::cout<<"****************"<<std::endl;
}
}
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
pcl::PointXYZI sampled_p;
pcl::PointCloud<pcl::PointXYZI> scan;
pcl::fromROSMsg(*input, scan);
if(measurement_range != MAX_MEASUREMENT_RANGE){
scan = removePointsByRange(scan, 0, measurement_range);
}
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
filtered_scan_ptr->header = scan.header;
int points_num = scan.size();
double w_total = 0.0;
double w_step = 0.0;
int m = 0;
double c = 0.0;
filter_start = std::chrono::system_clock::now();
for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin(); item != scan.end(); item++)
{
w_total += item->x * item->x + item->y * item->y + item->z * item->z;
}
w_step = w_total / sample_num;
pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin();
for (m = 0; m < sample_num; m++)
{
while (m * w_step > c)
{
item++;
c += item->x * item->x + item->y * item->y + item->z * item->z;
}
sampled_p.x = item->x;
sampled_p.y = item->y;
sampled_p.z = item->z;
sampled_p.intensity = item->intensity;
filtered_scan_ptr->points.push_back(sampled_p);
}
sensor_msgs::PointCloud2 filtered_msg;
pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);
filter_end = std::chrono::system_clock::now();
filtered_msg.header = input->header;
filtered_points_pub.publish(filtered_msg);
points_downsampler_info_msg.header = input->header;
points_downsampler_info_msg.filter_name = "distance_filter";
points_downsampler_info_msg.measurement_range = measurement_range;
points_downsampler_info_msg.original_points_size = points_num;
points_downsampler_info_msg.filtered_points_size = std::min((int)filtered_scan_ptr->size(), points_num);
points_downsampler_info_msg.original_ring_size = 0;
points_downsampler_info_msg.original_ring_size = 0;
points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0;
points_downsampler_info_pub.publish(points_downsampler_info_msg);
if(_output_log == true){
if(!ofs){
std::cerr << "Could not open " << filename << "." << std::endl;
exit(1);
}
ofs << points_downsampler_info_msg.header.seq << ","
<< points_downsampler_info_msg.header.stamp << ","
<< points_downsampler_info_msg.header.frame_id << ","
<< points_downsampler_info_msg.filter_name << ","
<< points_downsampler_info_msg.original_points_size << ","
<< points_downsampler_info_msg.filtered_points_size << ","
<< points_downsampler_info_msg.original_ring_size << ","
<< points_downsampler_info_msg.filtered_ring_size << ","
<< points_downsampler_info_msg.exe_time << ","
<< std::endl;
}
}