Commit 3b8d3506 authored by Marius's avatar Marius

asd

parent 1bee7cfc
This diff is collapsed.
......@@ -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
......@@ -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();
......
#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;
};
#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;
};
......@@ -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>();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment