asd

CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
         visualization_msgs
         sensor_msgs
         message_generation
+        image_transport
         pcl_ros
         cv_bridge
         tf
@@ -69,6 +70,8 @@ include_directories(${SDL2_INCLUDE_DIR})
 find_package(PCL REQUIRED COMPONENTS common io)
 include_directories({PCL_INCLUDE_DIRS})
 
+####################################### OpenCV #######################################
+find_package(OpenCV REQUIRED)
 
 ####################################### Comon #######################################
 set(comon_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/comon/)
@@ -272,3 +275,25 @@ add_executable(crs_registration_icp_student ${registration_icp_student_SOURCES}
 set_target_properties(crs_registration_icp_student PROPERTIES OUTPUT_NAME registration_icp_student PREFIX "")
 target_link_libraries(crs_registration_icp_student crs_comon ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
 
+
+####################################### image_processing_node #######################################
+set(image_processing_node_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/image_processing_node/)
+file(GLOB image_processing_node_HEADERS ${image_processing_node_INCLUDE_DIR}/*.h*)
+
+set(image_processing_node_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/image_processing_node/)
+file(GLOB image_processing_node_SOURCES ${image_processing_node_SOURCE_DIR}/*.c*)
+
+add_executable(crs_image_processing_node ${image_processing_node_SOURCES} ${image_processing_node_HEADERS})
+set_target_properties(crs_image_processing_node PROPERTIES OUTPUT_NAME image_processing_node PREFIX "")
+target_link_libraries(crs_image_processing_node crs_comon ${OpenCV_LIBS} ${catkin_LIBRARIES})
+
+####################################### image_processing_node_student #######################################
+set(image_processing_node_student_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/image_processing_node_student/)
+file(GLOB image_processing_node_student_HEADERS ${image_processing_node_student_INCLUDE_DIR}/*.h*)
+
+set(image_processing_node_student_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/image_processing_node_student/)
+file(GLOB image_processing_node_student_SOURCES ${image_processing_node_student_SOURCE_DIR}/*.c*)
+
+add_executable(crs_image_processing_node_student ${image_processing_node_student_SOURCES} ${image_processing_node_student_HEADERS})
+set_target_properties(crs_image_processing_node_student PROPERTIES OUTPUT_NAME image_processing_node_student PREFIX "")
+target_link_libraries(crs_image_processing_node_student crs_comon ${OpenCV_LIBS} ${catkin_LIBRARIES})
\ No newline at end of file

src/er1_driver_node/main.cpp

@@ -73,8 +73,10 @@ int main(int argc, char** argv) {
 
     // Connect to robot. You have to secify which robot you are using!
 
-	EvolutionRobotID id = EvolutionRobotID::ER1Medium;
-    robot.connect(id);
+	EvolutionRobotID id = EvolutionRobotID::ER1Large;
+    if (!robot.connect(id)) {
+        ROS_ERROR("Could not connect to robot");
+    }
 
     // Enter ROS main loop.
     ros::spin();

src/image_processing_node/main.cpp

+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <geometry_msgs/Pose.h>
+#include <chrono>
+#include <comon/clockhelper.hpp>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+
+std::string getImageType(int number)
+{
+    // find type
+    int imgTypeInt = number%8;
+    std::string imgTypeString;
+
+    switch (imgTypeInt)
+    {
+        case 0:
+            imgTypeString = "8U";
+            break;
+        case 1:
+            imgTypeString = "8S";
+            break;
+        case 2:
+            imgTypeString = "16U";
+            break;
+        case 3:
+            imgTypeString = "16S";
+            break;
+        case 4:
+            imgTypeString = "32S";
+            break;
+        case 5:
+            imgTypeString = "32F";
+            break;
+        case 6:
+            imgTypeString = "64F";
+            break;
+        default:
+            break;
+    }
+
+    // find channel
+    int channel = (number/8) + 1;
+
+    std::stringstream type;
+    type<<"CV_"<<imgTypeString<<"C"<<channel;
+
+    return type.str();
+}
+
+image_transport::Publisher pubGrey;
+image_transport::Publisher pubGreyEdges;
+image_transport::Publisher pubDepthEdges;
+image_transport::Publisher pubMedian;
+image_transport::Publisher pubH;
+image_transport::Publisher pubS;
+image_transport::Publisher pubV;
+
+// Less typing
+using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
+
+// We are going to publish clouds ... Therefore we need a publisher!
+ros::Publisher publisher;
+
+// We are going to receive clouds ... Therefore we need a subscriber!
+ros::Subscriber subscriber;
+
+sensor_msgs::ImagePtr createImageMsg(cv::Mat mat) {
+    cv_bridge::CvImage img;
+    img.image = mat;
+    auto type = mat.type();
+    std::string encoding;
+    switch (type) {
+        case CV_8UC1: encoding = sensor_msgs::image_encodings::MONO8; break;
+        case CV_8UC3: encoding = sensor_msgs::image_encodings::RGB8; break;
+        case CV_16UC1: encoding = sensor_msgs::image_encodings::MONO16; break;
+    }
+    img.encoding = encoding;
+    img.header.stamp = ros::Time::now();
+    return img.toImageMsg();
+}
+
+
+
+void depthCallback(const sensor_msgs::ImageConstPtr imgPtr) {
+    auto img = cv_bridge::toCvCopy(imgPtr);
+    ROS_INFO("image received of type %s", getImageType(img->image.type()).c_str());
+
+
+    double min, max;
+    cv::minMaxLoc(img->image, &min, &max);
+    cv::Mat grey;
+    img->image.convertTo(grey, CV_8UC1, 256.0 / max);
+
+    cv::minMaxLoc(grey, &min, &max);
+    ROS_INFO("Min %f Max %f", min, max);
+
+    cv::Mat depthEdges = grey.clone();
+
+    cv::Canny(grey, depthEdges, 100, 100);
+
+
+
+    auto greyEdgesMsg = createImageMsg(depthEdges);
+    pubDepthEdges.publish(greyEdgesMsg);
+
+
+
+}
+
+void colorCallback(const sensor_msgs::ImageConstPtr imgPtr) {
+    auto img = cv_bridge::toCvCopy(imgPtr);
+    ROS_INFO("image received of type %s", getImageType(img->image.type()).c_str());
+    cv::Mat grey;
+    cv::cvtColor(img->image, grey, cv::COLOR_RGB2GRAY);
+
+    cv::Mat hsv;
+    cv::cvtColor(img->image, hsv, cv::COLOR_BGR2HSV);
+
+    cv::Mat split[3];
+    cv::split(hsv, split);
+
+
+
+    pubH.publish(createImageMsg(split[0]));
+    pubS.publish(createImageMsg(split[1]));
+    pubV.publish(createImageMsg(split[2]));
+
+
+    cv::Mat greyEdges = grey.clone();
+    cv::Mat median = grey.clone();
+
+    cv::medianBlur(grey, median, 11);
+    cv::Canny(median, greyEdges,100, 100);
+
+
+    auto greyEdgesMsg = createImageMsg(greyEdges);
+    pubGreyEdges.publish(greyEdgesMsg);
+
+    auto greyMsg = createImageMsg(grey);
+    pubGrey.publish(greyMsg);
+
+    auto medianMsg = createImageMsg(median);
+    pubMedian.publish(medianMsg);
+}
+
+int main(int argc, char** argv){
+    // Init ROS stuff
+    ros::init(argc, argv, "crs_point_processing_node");
+    auto node = ros::NodeHandle();
+    image_transport::ImageTransport it(node);
+
+    image_transport::Subscriber subDepth = it.subscribe("/kinect2/sd/image_depth", 1, depthCallback);
+    image_transport::Subscriber subColor = it.subscribe("/kinect2/hd/image_color", 1, colorCallback);
+
+    pubGrey = it.advertise("/crs/grey", 1);
+    pubGreyEdges = it.advertise("/crs/grey_edges", 1);
+    pubMedian = it.advertise("/crs/grey_median", 1);
+
+    pubDepthEdges = it.advertise("/crs/depth_edges", 1);
+
+    pubH = it.advertise("/crs/H", 1);
+    pubS = it.advertise("/crs/S", 1);
+    pubV = it.advertise("/crs/V", 1);
+
+
+    ros::spin();
+
+    return 0;
+};

src/image_processing_node_student/main.cpp

+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <geometry_msgs/Pose.h>
+#include <chrono>
+#include <comon/clockhelper.hpp>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+
+
+// Less typing
+using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
+
+// We are going to publish clouds ... Therefore we need a publisher!
+ros::Publisher publisher;
+
+// We are going to receive clouds ... Therefore we need a subscriber!
+ros::Subscriber subscriber;
+
+
+// The callback function. It gets called whenever we receive a new point loucd.
+void callback(CloudRGBA::ConstPtr msgPtr) {
+    // Print some information
+    ROS_INFO("Received a cloud with %i points", msgPtr->size());
+
+
+    // Init voxelgrid filter and apply
+    pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
+    sor.setInputCloud(msgPtr);
+    sor.setLeafSize(0.01f, 0.01f, 0.01f);
+    auto filteredPtr = boost::make_shared<CloudRGBA>();
+    sor.filter(*filteredPtr);
+
+
+    // Set the header of the message. Just some shabby details ...
+    filteredPtr->header.frame_id = "filtered_frame";
+    pcl_conversions::toPCL(ros::Time::now(), filteredPtr->header.stamp);
+
+    // Publish cloud
+    publisher.publish(filteredPtr);
+
+    // Print some information
+    ROS_INFO("Published a cloud with %i points", filteredPtr->size());
+}
+
+
+
+
+
+
+int main(int argc, char** argv){
+    // Init ROS stuff
+    ros::init(argc, argv, "crs_tf_publisher");
+    auto node = ros::NodeHandle();
+
+    // Init subscriber and set topic
+    subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
+
+    // Init publisher and set topic
+
+    publisher = node.advertise<CloudRGBA>("/crs/filtered_vg", 5);
+
+
+    // Run!
+    ros::spin();
+
+    return 0;
+};

src/registration_icp_student/main.cpp

@@ -91,7 +91,7 @@ int main(int argc, char **argv) {
     // The ROS Bag Helper.
     BagFileHelper helper;
     helper.openBagFile(bagFilePath);
-    helper.initTopic(cloudSubscribeTopic);
+    helper.initTopic("");
 
     // Create an empty cloud that is going to contain ALL registered point clouds
     auto completeCloud = boost::make_shared<CloudRGBA>();