CleanSpotServer.cpp

Go to the documentation of this file.

#include <iostream>
#include <limits>
#include <cmath>
#include <cstdlib>
#include <map>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include <ros/ros.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>

#include <tf/tf.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <eigen3/Eigen/src/Core/EigenBase.h>
#include <eigen3/Eigen/src/Geometry/Quaternion.h>

#include <octomap_ros/OctomapROS.h>
#include <octomap/octomap.h>
#include <octomap_ros/conversions.h>

#include <surfacelet/SurfacePatch.h>
#include <surfacelet/SubmodMap.h>
//#include <surfacelet/PatchMap.h>
#include <coverage_3d_planning/TSPSearch.h>
#include <coverage_3d_arm_navigation/openrave_ik.h> //ik generated by openrave for the right arm
#include <coverage_3d_arm_navigation/JointSpaceArmController.h>
#include <coverage_3d_planning/NeighborhoodGraph.h>
#include <coverage_3d_tools/conversions.h>
#include <coverage_3d_planning/TSPSolver.h>
#include <coverage_3d_planning/GTSPSolver.h>
#include <coverage_3d_planning/JointGTSPSolver.h>
#include <coverage_3d_srvs/CleanSpot.h>
#include <coverage_3d_tools/filter.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_types.h>
#include <tf/transform_listener.h>
#include <Eigen/Core>
#include <boost/algorithm/string.hpp>
#include <std_srvs/Empty.h>
#include <tf/transform_listener.h>
#include <boost/thread.hpp>
#include <boost/format.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <coverage_3d_tools/AssembleCloud.h>
#include <pcl_ros/transforms.h>

using namespace std;
namespace coverage_3d_executive {

class CleanSpotServer {

public:
    CleanSpotServer(ros::NodeHandle& n);
    ~CleanSpotServer();
    void cloudCB(const sensor_msgs::PointCloud2ConstPtr& msg);

private:
    ros::NodeHandle nh_;
    ros::ServiceServer srv_;
    ros::Subscriber cloud_sub_;
    ros::Publisher cloud_pub_;
    tf::TransformListener tf_;

    ros::Publisher pub_cloud_used_;
    ros::Publisher pub_cloud_not_used_;
    ros::Publisher pub_patches_;
    ros::Publisher pub_normals_;
    ros::Publisher pub_nhgraph_;
    ros::Publisher pub_solution_;

    pcl::PointCloud<pcl::PointXYZ> assembled_cloud_;

    bool srvCB(coverage_3d_srvs::CleanSpot::Request& req,
        coverage_3d_srvs::CleanSpot::Response& resp);
    void runThreadCallbackLoop();
    void constructSolutionMarker(std::vector<pcl::PointXYZ> surface_samples,
            const std::vector<std::vector<unsigned int> >& solutions, const std::string frame_id,
            std::vector<visualization_msgs::Marker>& marker_list);
    void constructPathMarker(const std::vector<tf::Pose>& poses, const std::string frame_id,
            visualization_msgs::MarkerArray& marker_list);
    void endEffectorPose(const SurfacePatch p1, const double dist_above_surface, tf::Pose& pose);

    ros::NodeHandle cloud_nh_;
    boost::thread * cloud_thread_;
    bool cloud_thread_shutdown_;
    bool cloud_received_;
    double box_size_;
    JointSpaceArmController* arm_control_;

    ros::CallbackQueue cloud_callback_queue_;
    boost::recursive_mutex cloud_mutex_;
    tf::TransformListener listener;
    pcl::PointXYZ cloud_view_point_;
    tf::Point  spot_to_clean_;
    pcl::PointCloud<pcl::PointXYZ> cloud_;
    pcl::PointCloud<pcl::PointXYZ> cloud_nans_removed_;
    pcl::PointCloud<pcl::PointXYZ> cloud_not_used_;
    std::vector<double> start_joint_pose_;
    tf::Pose start_tf_pose_;


};

CleanSpotServer::CleanSpotServer(ros::NodeHandle& n) :
        nh_(n), cloud_thread_shutdown_(true),cloud_received_(false),box_size_(0.3),arm_control_(0){
    cloud_nh_.setCallbackQueue(&cloud_callback_queue_);
    srv_ = nh_.advertiseService("clean_spot", &CleanSpotServer::srvCB,
            this);
    cloud_pub_=nh_.advertise<sensor_msgs::PointCloud2>("build_cloud_msg",2);

    pub_cloud_used_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_used", 1);
    pub_cloud_not_used_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_not_used", 1);
    pub_patches_ = nh_.advertise<visualization_msgs::MarkerArray>("surface_patches", 1);
    pub_normals_ = nh_.advertise<visualization_msgs::MarkerArray>("surface_normals", 1);
    pub_nhgraph_= nh_.advertise<visualization_msgs::Marker>("neighborhood_graph", 1);
    pub_solution_ = nh_.advertise<visualization_msgs::Marker>("tsp_solution", 1);

    //set the arm start poses
    start_joint_pose_.resize(7);
    start_joint_pose_[0] = -1.68;
    start_joint_pose_[1] = 0.052;
    start_joint_pose_[2] = -1.48;
    start_joint_pose_[3] = -1.65;
    start_joint_pose_[4] = -1.60;
    start_joint_pose_[5] = -1.57;
    start_joint_pose_[6] = -3.02;

    start_tf_pose_.setOrigin(tf::Point(0.278, -0.710, -0.158));

 //   arm_control_ = new  JointSpaceArmController(nh_);

    cloud_view_point_ = pcl::PointXYZ(0,0,0);
}
CleanSpotServer::~CleanSpotServer() {
        if (arm_control_!=0){
                delete arm_control_;
        }
}

void CleanSpotServer::runThreadCallbackLoop() {
    ros::Rate r(15);
    while (ros::ok() && !cloud_thread_shutdown_) {
        ROS_INFO("Listening for pointclouds ");
        cloud_callback_queue_.callAvailable(ros::WallDuration()); // call all available callbacks
        r.sleep();
    }
}


bool CleanSpotServer::srvCB(coverage_3d_srvs::CleanSpot::Request& req,
    coverage_3d_srvs::CleanSpot::Response& resp) {
        arm_control_ = new  JointSpaceArmController(nh_);
    box_size_=req.box_size/2.0;
//    std::vector<tf::Pose> tmp;
//    arm_control_->moveArmToJointGoalUsingPlanner("right_arm", start_joint_pose_, false, tmp);

    //transforming the received cleaning spot to our frame
    ROS_INFO("Transforming spot received to torso_lift_link");
    tf::StampedTransform transform_kinect_torso;
    //get transform to torso_lift_link
    bool transform_received = false;
    while (!transform_received && ros::ok()) {
        try {
            ros::Duration timeout(15.0 / 30.0);
            tf_.waitForTransform("/torso_lift_link", req.spot.header.frame_id,ros::Time(0), timeout);
            tf_.lookupTransform("/torso_lift_link", req.spot.header.frame_id, ros::Time(0), transform_kinect_torso);
            transform_received = true;
        } catch (exception e) {
            ROS_WARN_STREAM(e.what());
        }ROS_INFO( "Continue waiting for transform.");
        ros::Rate r(10);
        r.sleep();
    }

    tf::Point received_spot;
    tf::pointMsgToTF(req.spot.point,received_spot);
    spot_to_clean_= transform_kinect_torso*received_spot;


    //getting the pointcloud
    cloud_received_ = false;
    cloud_sub_ = nh_.subscribe("/kinect_head/depth_registered/points", 100,
            &CleanSpotServer::cloudCB, this);
    //cloud_thread_shutdown_ = false;
    //cloud_thread_ = new boost::thread(
     //       &CleanSpotServer::runThreadCallbackLoop, this);

    //wait for point cloud
    ros::Rate loop_rate(10);
    while (!cloud_received_) {
        ROS_INFO("Waiting for pointcloud.");
        loop_rate.sleep();
    }
    cloud_thread_shutdown_ = true;
    //delete (cloud_thread_);
    cloud_sub_.shutdown();
    ROS_INFO("Pointcloud received.");

        //cloud_mutex_.lock();
    //Patch construction and publishing
//    surfacelet::PatchMap map;
    surfacelet::SubmodMap map;
    pcl::PointCloud<pcl::PointXYZ> not_used_cloud;
    map.insertScan(cloud_nans_removed_, not_used_cloud, cloud_view_point_);

    sensor_msgs::PointCloud2 not_used_msg;
    pcl::toROSMsg(not_used_cloud, not_used_msg);
    not_used_msg.header.stamp = ros::Time::now();
    not_used_msg.header.frame_id = cloud_.header.frame_id;
    pub_cloud_not_used_.publish(not_used_msg);

    sensor_msgs::PointCloud2 msg;
    pcl::toROSMsg(cloud_nans_removed_, msg);
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = cloud_.header.frame_id;
    pub_cloud_used_.publish(msg);

//    if (cloud_nans_removed_.size()==0){
//      ROS_ERROR("No valid patches found for wiping");
//      return false;
//    }

    //Octomap construction
    ROS_INFO("Constructing Octomap");
    octomap::OctomapROS<octomap::OcTree> octomap(0.01);
    octomap.octree.setOccupancyThres(0.6);
    octomap.insertScan(cloud_nans_removed_, cloud_view_point_);



    ROS_INFO("Constructing Neigborhood Graph Object");
    std::vector<SurfacePatch> g_patches;
    std::vector<pcl::PointXYZ> g_surface_points;
    Eigen::MatrixXi adjacency;
    std::vector<std::vector<tf::Pose> > coll_free_poses;
    std::vector<std::vector<std::vector<std::vector<double> > > > coll_free_ik;
    std::vector<bool> reachable;

    {
      ros::NodeHandle   nh("zweitnodehandle");

    NeighborhoodGraph nh_graph(nh, map.map_, &octomap);
    nh_graph.setSurfaceDist(0.03);

    nh_graph.getGraph(g_patches, g_surface_points, adjacency, coll_free_poses, coll_free_ik);

    nh_graph.getReachableVector(reachable);
    }

    //return true;

    if (g_patches.size()==0){
      ROS_ERROR("No valid patches found for wiping");
      return false;
    }  

    std::vector<tf::Pose> all_poses;
    all_poses.clear();
//    int test = 0;
    for (int i = 0; i < adjacency.rows(); ++i) {
        for (unsigned int j = 0; j < coll_free_poses[i].size(); ++j) {
            all_poses.push_back(coll_free_poses[i][j]);
            geometry_msgs::Pose geom_pose;
            tf::poseTFToMsg(coll_free_poses[i][j], geom_pose);
        }
//        test = test + coll_free_poses[i].size() - adjacency.row(i).array().sum();
    }

//    visualization_msgs::MarkerArray all_poses_markers;
//    constructPathMarker(all_poses, cloud_.header.frame_id, all_poses_markers);


    //search TSP solution for each graph
    std::vector<std::vector<unsigned int> > solutions;
//  TSPSolver tsp;
    GTSPSolver tsp;
    tsp.setStartPose(start_joint_pose_, start_tf_pose_);
    tsp.setGraph(g_patches, g_surface_points, adjacency, coll_free_poses, coll_free_ik);
    std::vector<std::vector<double> > res_joint_path_tsp;
    std::vector<tf::Pose> res_pose_path_tsp;
    std::vector<unsigned int> res_actions_tsp;
    std::vector<unsigned int> res_traj_tsp;
    tsp.getSolution(res_joint_path_tsp, res_pose_path_tsp, res_actions_tsp, res_traj_tsp);

    //publish patches
    visualization_msgs::MarkerArray array;
    map.constructMarker(cloud_.header.frame_id, reachable, array);
    pub_patches_.publish(array);

    //publish patch normals
    visualization_msgs::MarkerArray normal_marker;
    map.constructNormalMarker(cloud_.header.frame_id, normal_marker);
    pub_normals_.publish(normal_marker);

    //publish neighborhood graph
//    visualization_msgs::Marker graph_marker;
 //   nh_graph.constructGraphMarker(g_surface_points, adjacency, cloud_.header.frame_id, graph_marker);
  //  graph_marker.lifetime= ros::Duration(5.0);
   // pub_nhgraph_.publish(graph_marker);

    //publish solution
    ROS_INFO("Constructing solution markers");
    std::vector<visualization_msgs::Marker> solutions_marker;
    solutions.push_back(res_traj_tsp);
    constructSolutionMarker(g_surface_points, solutions, cloud_.header.frame_id, solutions_marker);

    //publish resulting path as marker
    for (unsigned int i = 0; i < solutions.size(); i++) {
        pub_solution_.publish(solutions_marker[i]);
        ros::Duration(0.5).sleep();
    }

    std::vector<tf::Pose> touch_points_allowed;
    touch_points_allowed.push_back(start_tf_pose_);
    for (unsigned int i = 0; i < res_traj_tsp.size(); ++i) {
        tf::Pose tmp; //need another vector as it is cleared in the function
        endEffectorPose(g_patches[res_traj_tsp[i]], 0.00, tmp);
        touch_points_allowed.push_back(tmp);
    }
    touch_points_allowed.push_back(start_tf_pose_);

    std::vector<tf::Pose> poses_above_surface;
    poses_above_surface.push_back(start_tf_pose_);
    for (unsigned int i = 0; i < g_patches.size(); ++i) {
        tf::Pose tmp; //need another vector as it is cleared in the function
        endEffectorPose(g_patches[res_traj_tsp[i]], 0.30, tmp);
        poses_above_surface.push_back(tmp);
    }
    poses_above_surface.push_back(start_tf_pose_);

    ROS_ERROR_STREAM("Size of trajectories " << res_traj_tsp.size() << " " << touch_points_allowed.size() << " " << poses_above_surface.size() << " "
            << res_joint_path_tsp.size() << " " << res_pose_path_tsp.size() << " " << res_actions_tsp.size());

  bool cleaning_succeeded = arm_control_->followEntireTrajectory(res_joint_path_tsp, "right_arm", res_actions_tsp, res_pose_path_tsp, touch_points_allowed,
          poses_above_surface);
  ROS_INFO("FINISHED CLEANING");
  delete(arm_control_);
  //ROS_ASSERT(false);
        //cloud_mutex_.unlock();
    return cleaning_succeeded;
}

void CleanSpotServer::cloudCB(const sensor_msgs::PointCloud2ConstPtr& msg) {

        if (!cloud_received_) {

                tf::StampedTransform transform_kinect_torso;
                //get transform to torso_lift_link
                bool transform_received = false;
                while (!transform_received && ros::ok()) {
                        ros::Time lookup_time = ros::Time::now();
                        try {
                                ros::Duration timeout(15.0 / 30.0);
                                tf_.waitForTransform("/torso_lift_link", msg->header.frame_id, lookup_time, timeout);
                                tf_.lookupTransform("/torso_lift_link", msg->header.frame_id, lookup_time, transform_kinect_torso);
                                transform_received = true;
                        } catch (exception e) {
                                ROS_WARN_STREAM(e.what());
                        }ROS_INFO( "Continue waiting for transform.");
                        ros::Rate r(10);
                        r.sleep();
                }

                pcl::PointCloud<pcl::PointXYZ> tmp_cloud;
                pcl::fromROSMsg(*msg, tmp_cloud);
                pcl_ros::transformPointCloud(tmp_cloud, cloud_, transform_kinect_torso);
                cloud_.header.frame_id = "/torso_lift_link";

                pcl::PointCloud<pcl::PointXYZ> tmp_cloud2;
                filterXYZ(cloud_, tmp_cloud2, spot_to_clean_.getX()-box_size_, spot_to_clean_.getX()+box_size_, spot_to_clean_.getY()-box_size_, spot_to_clean_.getY()+box_size_, spot_to_clean_.getZ()-box_size_, spot_to_clean_.getZ()+box_size_);
    
//              filterXYZ(cloud_, tmp_cloud2, -1.2, 1.2, -0.3, 0.3, -1.0, 0.7);
                cloud_nans_removed_.points.clear();
    for (unsigned int i = 0; i < tmp_cloud2.points.size(); ++i) {
                        if (!pcl_isnan (tmp_cloud2.points[i].x) && !pcl_isnan (tmp_cloud2.points[i].y)
                                        && !pcl_isnan (tmp_cloud2.points[i].z)) {
                                cloud_nans_removed_.points.push_back(tmp_cloud2.points[i]);
                        }
                }
                cloud_nans_removed_.is_dense = false;
                cloud_nans_removed_.height = 1;
                cloud_nans_removed_.width = cloud_nans_removed_.points.size();

                pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
                sor.setInputCloud(cloud_nans_removed_.makeShared());
                sor.setMeanK(50);
                sor.setStddevMulThresh(1.0);
                pcl::PointCloud<pcl::PointXYZ> tmp_cloud3;
                sor.filter(tmp_cloud3);
                cloud_nans_removed_ = tmp_cloud3;

                tf::Point org_view_point(0, 0, 0);
                org_view_point = transform_kinect_torso * org_view_point;
                cloud_view_point_.x = org_view_point.x();
                cloud_view_point_.y = org_view_point.y();
                cloud_view_point_.z = org_view_point.z();
                ROS_DEBUG_STREAM("View Point " << cloud_view_point_.x << " " << cloud_view_point_.y << " " << cloud_view_point_.z);
                ROS_DEBUG_STREAM("Original View Point " <<org_view_point.x() << " " << org_view_point.y() << " " << org_view_point.z());
                cloud_received_ = true;
        }
}

void CleanSpotServer::constructSolutionMarker(std::vector<pcl::PointXYZ> surface_samples,
                const std::vector<std::vector<unsigned int> >& solutions, const std::string frame_id,
                std::vector<visualization_msgs::Marker>& marker_list) {

        marker_list.clear();
        visualization_msgs::Marker marker;

        // Set the frame ID and timestamp.  See the TF tutorials for information on these.
        marker.header.frame_id = frame_id;
        marker.header.stamp = ros::Time::now();

        // Set the namespace and id for this marker.  This serves to create a unique ID
        // Any marker sent with the same namespace and id will overwrite the old one
        marker.ns = "coverage";

        // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
        marker.type = visualization_msgs::Marker::LINE_LIST;

        // Set the marker action.  Options are ADD and DELETE
        marker.action = visualization_msgs::Marker::MODIFY;

        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        marker.scale.x = 0.005;
        marker.scale.y = 0.005;
        marker.scale.z = 0.005;

        // Set the color -- be sure to set alpha to something non-zero!
        marker.color.r = 1;
        marker.color.g = 1;
        marker.color.b = 0.5;
        marker.color.a = 1.0;

        geometry_msgs::Point start;
        geometry_msgs::Point end;

        for (unsigned int i = 0; i < solutions.size(); i++) {
                marker.id = i;
                marker.points.clear();

                for (unsigned int j = 0; j < solutions[i].size() - 1; j++) {
                        start.x = surface_samples[solutions[i][j]].x;
                        start.y = surface_samples[solutions[i][j]].y;
                        start.z = surface_samples[solutions[i][j]].z;
                        end.x = surface_samples[solutions[i][j + 1]].x;
                        end.y = surface_samples[solutions[i][j + 1]].y;
                        end.z = surface_samples[solutions[i][j + 1]].z;

                        marker.points.push_back(start);
                        marker.points.push_back(end);
                }
                marker_list.push_back(marker);
        }

}

void CleanSpotServer::constructPathMarker(const std::vector<tf::Pose>& poses, const std::string frame_id,
                visualization_msgs::MarkerArray& marker_list) {

        marker_list.markers.clear();
        for (unsigned int i = 0; i < poses.size(); i++) {

                visualization_msgs::Marker marker;

                // Set the frame ID and timestamp.  See the TF tutorials for information on these.
                marker.header.frame_id = frame_id;
                marker.header.stamp = ros::Time::now() - ros::Duration(0.5);

                // Set the namespace and id for this marker.  This serves to create a unique ID
                // Any marker sent with the same namespace and id will overwrite the old one
                marker.ns = "coverage";

                // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
                marker.type = visualization_msgs::Marker::ARROW;

                // Set the marker action.  Options are ADD and DELETE
                marker.action = visualization_msgs::Marker::MODIFY;

                // Set the scale of the marker -- 1x1x1 here means 1m on a side
                marker.scale.x = 0.01;
                marker.scale.y = 0.05;
                marker.scale.z = 0.03;

                // Set the color -- be sure to set alpha to something non-zero!
                marker.color.r = 0.0;
                marker.color.g = 0.0;
                marker.color.b = 0.9;
                marker.color.a = 1.0;

                marker.id = i;
                tf::poseTFToMsg(poses[i], marker.pose);
                marker_list.markers.push_back(marker);
        }

}

// for a single pose we sample end-effector rotations
void CleanSpotServer::endEffectorPose(const SurfacePatch p1, const double dist_above_surface, tf::Pose& pose) {
        //compute point above surface
        Eigen::Vector3f origin = p1.getVector3fMap() + (dist_above_surface * p1.getNormalVector3fMap().normalized())
                        + ((p1.scale_z / 2.0) * p1.getNormalVector3fMap().normalized());

        //set x_axis to point to surface
        Eigen::Vector3f z_axis = p1.getVector3fMap() - origin;
        z_axis.normalize();

        //compute angle-axis representation
        //compute cross product of normal and z-axis and get the rotation with smallest angle
        //cross product is the rotation axis with smallest angle
        Eigen::Vector3f cross_product = Eigen::Vector3f(1, 0, 0).cross(z_axis);
        Eigen::Matrix3f rot_matrix;
        cross_product.normalize();
        float angle = 0;
        if (cross_product.norm() > 0.000000001) { //everything ok
                angle = acos(z_axis.dot(Eigen::Vector3f(1, 0, 0)));
                rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
        } else { //cross product 0 - normal either in or exactly opposite of z-axis
                double tmp = z_axis.dot(Eigen::Vector3f(1, 0, 0));
                if (tmp > 0) {
                        rot_matrix = Eigen::Matrix3f::Identity();
                } else {
                        angle = M_PI;
                        rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
                }
        }

        //set everything into the pose
        pose.setOrigin(tf::Point(origin[0], origin[1], origin[2]));
        Eigen::Quaternion<float> quat(rot_matrix);
        tf::Quaternion tf_quat(quat.x(), quat.y(), quat.z(), quat.w());
        pose.setRotation(tf_quat);
}
};
int main(int argc, char** argv) {

    ros::init(argc, argv, "clean_spot_server");
    ros::NodeHandle n;
    coverage_3d_executive::CleanSpotServer server(n);
    ros::Rate r(15);

    ros::AsyncSpinner spinner(4); // Use 4 threads
    spinner.start();
    ros::waitForShutdown();
//    while (ros::ok()) {
//        ROS_INFO("Spining");
//        ros::spinOnce();
//        r.sleep();
//    }

    //ros::spin();

}