Commit a613f5a3 authored by Marius's avatar Marius

Merge remote-tracking branch 'origin/master'

# Conflicts:
#	src/er1_driver_node/main.cpp
#	src/point_processing_node/main.cpp
#	src/point_processing_node_student/main.cpp
parents 25e3629a 9729ce5b
......@@ -72,7 +72,7 @@ int main(int argc, char** argv) {
ros::Subscriber sub = node.subscribe("er1_motor_commands", 1000, &callback);
// Connect to robot. You have to secify which robot you are using!
EvolutionRobotID id = EvolutionRobotID::ER1Medium;
EvolutionRobotID id = EvolutionRobotID::ER1Small;
robot.connect(id);
// Enter ROS main loop.
......
......@@ -3,59 +3,47 @@
#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());
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
// 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);
// Create Transform Broadcaster
tf::TransformBroadcaster br;
// Init the clock helper
ClockHelper helper;
helper.init();
helper.setCycleTime(std::chrono::seconds(3));
// 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);
// Specify an update rate
ros::Rate rate(3);
// Publish cloud
publisher.publish(filteredPtr);
// Main loop
while(ros::ok()) {
// Get the roll value for the current time point
auto roll = helper.getRoll();
// Print some information
ROS_INFO("Published a cloud with %i points", filteredPtr->size());
}
// Specify the origin for our transformation
auto origin = tf::Vector3(0, 0, 0);
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
auto node = ros::NodeHandle();
// Specify the rotation for our transformation
tf::Quaternion q;
q.setRPY(0, 0, roll);
// Init subscriber and set topic
subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
// Bundle all information into a transformation object
tf::Transform transform;
transform.setOrigin(origin);
transform.setRotation(q);
// Init publisher and set topic
publisher = node.advertise<CloudRGBA>("/crs/points", 5);
// Publish the transformation
std::string destinationFrame = "map";
std::string sourceFrame = "kinect2_ir_optical_frame";
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), destinationFrame, sourceFrame));
// Run!
ros::spin();
// Sleep a little bit
rate.sleep();
}
return 0;
};
\ No newline at end of file
......@@ -24,20 +24,20 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
ROS_INFO("Received a cloud with %i points", msgPtr->size());
auto filteredPtr = boost::make_shared<CloudRGBA>();
// Init voxelgrid filter and apply
pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
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());
......@@ -45,11 +45,14 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_cloud_processor");
ros::init(argc, argv, "crs_tf_publisher");
auto node = ros::NodeHandle();
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
// 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();
......
......@@ -36,8 +36,6 @@ int main(int argc, char** argv){
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
// Publish the transformation
std::string destinationFrame;
/* ######### <MISSING> ########## */
......@@ -52,4 +50,4 @@ int main(int argc, char** argv){
}
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