Commit c808bc26 authored by Marius's avatar Marius

just commit

asd
parent e27f4fb9
This diff is collapsed.
......@@ -72,7 +72,8 @@ 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::ER1Small;
EvolutionRobotID id = EvolutionRobotID::ER1Medium;
robot.connect(id);
// Enter ROS main loop.
......
......@@ -3,47 +3,67 @@
#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>
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
// Create Transform Broadcaster
tf::TransformBroadcaster br;
// 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);
// 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());
}
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
auto node = ros::NodeHandle();
// Specify the origin for our transformation
auto origin = tf::Vector3(0, 0, 0);
// Init subscriber and set topic
subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
// Specify the rotation for our transformation
tf::Quaternion q;
q.setRPY(0, 0, roll);
// Init publisher and set topic
// Bundle all information into a transformation object
tf::Transform transform;
transform.setOrigin(origin);
transform.setRotation(q);
publisher = node.advertise<CloudRGBA>("/crs/filtered_vg", 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));
// Sleep a little bit
rate.sleep();
}
// Run!
ros::spin();
return 0;
};
\ No newline at end of file
};
......@@ -7,8 +7,9 @@
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
// Less typing
// Less typing. We are using a pointcloud that contains XYZ and addtionally RGBA data.
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
// We are going to publish clouds ... Therefore we need a publisher!
......@@ -24,20 +25,23 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
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);
pcl::VoxelGrid<pcl::PointXYZRGBA> voxelFilter;
voxelFilter.setInputCloud (msgPtr);
/* ######### <MISSING> ########## */
auto filteredPtr = boost::make_shared<CloudRGBA>();
sor.filter(*filteredPtr);
voxelFilter.filter(*filteredPtr);
// Set the header of the message. Just some shabby details ...
filteredPtr->header.frame_id = "filtered_frame";
filteredPtr->header.frame_id = ""; /* ######### <MISSING> ########## */
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,17 +49,28 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
ros::init(argc, argv, "crs_cloud_processor");
auto node = ros::NodeHandle();
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
std::string subscribeTopic;
/* ######### <MISSING> ########## */
std::string advertiseTopic;
/* ######### <MISSING> ########## */
// Init subscriber and set topic
subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
subscriber = node.subscribe<CloudRGBA>(subscribeTopic, 1, callback);
// Init publisher and set topic
publisher = node.advertise<CloudRGBA>("/crs/points", 5);
publisher = node.advertise<CloudRGBA>(advertiseTopic, 5);
// Run!
ros::spin();
/* ######### <MISSING> ########## */
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