Commit b91c59ca authored by Marius's avatar Marius

wer

parent e86e1f66
<?xml version="1.0" encoding="UTF-8"?>
<module classpath="CMake" type="CPP_MODULE" version="4" />
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="Encoding" addBOMForNewFiles="with NO BOM" />
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
<component name="JavaScriptSettings">
<option name="languageLevel" value="ES6" />
</component>
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/crs.iml" filepath="$PROJECT_DIR$/.idea/crs.iml" />
</modules>
</component>
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$" vcs="Git" />
</component>
</project>
\ No newline at end of file
This diff is collapsed.
......@@ -26,6 +26,7 @@ find_package(catkin REQUIRED COMPONENTS
message_generation
pcl_ros
cv_bridge
tf
)
add_message_files(
......@@ -37,6 +38,7 @@ add_message_files(
generate_messages(
DEPENDENCIES
std_msgs
geometry_msgs
)
catkin_package(
......@@ -50,6 +52,8 @@ include_directories(
${catkin_INCLUDE_DIRS}
)
####################################### Boost #######################################
FIND_PACKAGE(Boost REQUIRED filesystem thread system graph)
include_directories(${Boost_INCLUDE_DIRS})
......@@ -61,6 +65,10 @@ set(SDL2_LIBRARY /usr/lib/x86_64-linux-gnu/libSDL2-2.0.so)
set(SDL2_INCLUDE_DIR /usr/include/})
include_directories(${SDL2_INCLUDE_DIR})
####################################### PCL #######################################
find_package(PCL REQUIRED COMPONENTS common io)
include_directories({PCL_INCLUDE_DIRS})
####################################### Comon #######################################
set(comon_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/comon/)
......@@ -197,5 +205,48 @@ target_link_libraries(crs_er1_control_node crs_er1driver ${SDL2_LIBRARY} ${catki
####################################### tf_publisher_node #######################################
set(tf_publisher_node_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/tf_publisher_node/)
file(GLOB tf_publisher_node_HEADERS ${tf_publisher_node_INCLUDE_DIR}/*.h*)
set(tf_publisher_node_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/tf_publisher_node/)
file(GLOB tf_publisher_node_SOURCES ${tf_publisher_node_SOURCE_DIR}/*.c*)
add_executable(crs_tf_publisher_node ${tf_publisher_node_SOURCES} ${tf_publisher_node_HEADERS})
set_target_properties(crs_tf_publisher_node PROPERTIES OUTPUT_NAME tf_publisher_node PREFIX "")
target_link_libraries(crs_tf_publisher_node crs_comon ${catkin_LIBRARIES})
####################################### tf_publisher_node_student #######################################
set(tf_publisher_node_student_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/tf_publisher_node_student/)
file(GLOB tf_publisher_node_student_HEADERS ${tf_publisher_node_student_INCLUDE_DIR}/*.h*)
set(tf_publisher_node_student_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/tf_publisher_node_student/)
file(GLOB tf_publisher_node_student_SOURCES ${tf_publisher_node_student_SOURCE_DIR}/*.c*)
add_executable(crs_tf_publisher_node_student ${tf_publisher_node_student_SOURCES} ${tf_publisher_node_student_HEADERS})
set_target_properties(crs_tf_publisher_node_student PROPERTIES OUTPUT_NAME tf_publisher_node_student PREFIX "")
target_link_libraries(crs_tf_publisher_node_student crs_comon ${catkin_LIBRARIES})
####################################### point_processing_node #######################################
set(point_processing_node_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/point_processing_node/)
file(GLOB point_processing_node_HEADERS ${point_processing_node_INCLUDE_DIR}/*.h*)
set(point_processing_node_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/point_processing_node/)
file(GLOB point_processing_node_SOURCES ${point_processing_node_SOURCE_DIR}/*.c*)
add_executable(crs_point_processing_node ${point_processing_node_SOURCES} ${point_processing_node_HEADERS})
set_target_properties(crs_point_processing_node PROPERTIES OUTPUT_NAME point_processing_node PREFIX "")
target_link_libraries(crs_point_processing_node crs_comon ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
####################################### point_processing_node_student #######################################
set(point_processing_node_student_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/point_processing_node_student/)
file(GLOB point_processing_node_student_HEADERS ${point_processing_node_student_INCLUDE_DIR}/*.h*)
set(point_processing_node_student_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/point_processing_node_student/)
file(GLOB point_processing_node_student_SOURCES ${point_processing_node_student_SOURCE_DIR}/*.c*)
add_executable(crs_point_processing_node_student ${point_processing_node_student_SOURCES} ${point_processing_node_student_HEADERS})
set_target_properties(crs_point_processing_node_student PROPERTIES OUTPUT_NAME point_processing_node_student PREFIX "")
target_link_libraries(crs_point_processing_node_student crs_comon ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
#pragma once
#include <chrono>
class ClockHelper {
public:
void init();
void setCycleTime(std::chrono::seconds secs);
double getRoll();
private:
std::chrono::system_clock::time_point start_ = std::chrono::system_clock::now();
std::chrono::seconds cycleTime_ = std::chrono::seconds(4);
};
\ No newline at end of file
#include <comon/clockhelper.hpp>
constexpr double pi = 3.14159265358979323846;
void ClockHelper::init() {
start_ = std::chrono::system_clock::now();
}
void ClockHelper::setCycleTime(std::chrono::seconds secs) {
cycleTime_ = secs;
}
double ClockHelper::getRoll() {
auto now = std::chrono::system_clock::now();
auto diffInMS = std::chrono::duration_cast<std::chrono::milliseconds>(now - start_);
double step = 2 * pi / (1000 * cycleTime_.count());
auto ms = diffInMS.count() % (cycleTime_.count() * 1000);
return ms * step;
}
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Pose.h>
#include <chrono>
#include <comon/clockhelper.hpp>
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
// Create Transform Broadcaster
tf::TransformBroadcaster br;
// Init the clock helper
ClockHelper helper;
helper.init();
helper.setCycleTime(std::chrono::seconds(3));
// Specify an update rate
ros::Rate rate(3);
// Main loop
while(ros::ok()) {
// Get the roll value for the current time point
auto roll = helper.getRoll();
// Specify the origin for our transformation
auto origin = tf::Vector3(0, 0, 0);
// Specify the rotation for our transformation
tf::Quaternion q;
q.setRPY(0, 0, roll);
// Bundle all information into a transformation object
tf::Transform transform;
transform.setOrigin(origin);
transform.setRotation(q);
// Publish the transformation
std::string destinationFrame = "map";
std::string sourceFrame = "kinect2_ir_optical_frame";
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), destinationFrame, sourceFrame));
// Sleep a little bit
rate.sleep();
}
return 0;
};
\ No newline at end of file
#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/points", 5);
// Run!
ros::spin();
return 0;
};
\ No newline at end of file
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Pose.h>
#include <chrono>
#include <comon/clockhelper.hpp>
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
// Create Transform Broadcaster
tf::TransformBroadcaster br;
// Init the clock helper
ClockHelper helper;
helper.init();
helper.setCycleTime(std::chrono::seconds(3));
// Specify an update rate
ros::Rate rate(3);
// Main loop
while(ros::ok()) {
// Get the roll value for the current time point
auto roll = helper.getRoll();
// Specify the origin for our transformation
auto origin = tf::Vector3(0, 0, 0);
// Specify the rotation for our transformation
tf::Quaternion q;
q.setRPY(0, 0, roll);
// Bundle all information into a transformation object
tf::Transform transform;
transform.setOrigin(origin);
transform.setRotation(q);
// Publish the transformation
std::string destinationFrame = "map";
std::string sourceFrame = "kinect2_ir_optical_frame";
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), destinationFrame, sourceFrame));
// Sleep a little bit
rate.sleep();
}
return 0;
};
\ No newline at end of file
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Pose.h>
#include <chrono>
#include <comon/clockhelper.hpp>
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
// Create Transform Broadcaster
tf::TransformBroadcaster br;
// Init the clock helper
ClockHelper helper;
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
// Specify an update rate
ros::Rate rate(3);
// Main loop
while(ros::ok()) {
// Specify the origin for our transformation
tf::Vector3 origin;
/* ######### <MISSING> ########## */
// Specify the rotation for our transformation
tf::Quaternion q;
/* ######### <MISSING> ########## */
// Bundle all information into a transformation object
tf::Transform transform;
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
// Publish the transformation
std::string destinationFrame;
/* ######### <MISSING> ########## */
std::string sourceFrame;
/* ######### <MISSING> ########## */
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), destinationFrame, sourceFrame));
// Sleep a little bit
rate.sleep();
}
return 0;
};
\ No newline at end of file
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