Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
What's new
7
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Open sidebar
Marius Bock
crs
Commits
56486826
Commit
56486826
authored
Jan 09, 2019
by
Marius
Browse files
Options
Browse Files
Download
Plain Diff
Merge branch 'master' into testbranch
parents
25e3629a
a613f5a3
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
45 additions
and
56 deletions
+45
-56
src/er1_driver_node/main.cpp
src/er1_driver_node/main.cpp
+1
-1
src/point_processing_node/main.cpp
src/point_processing_node/main.cpp
+31
-43
src/point_processing_node_student/main.cpp
src/point_processing_node_student/main.cpp
+12
-9
src/tf_publisher_node_student/main.cpp
src/tf_publisher_node_student/main.cpp
+1
-3
No files found.
src/er1_driver_node/main.cpp
View file @
56486826
...
...
@@ -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
::
ER1
Medium
;
EvolutionRobotID
id
=
EvolutionRobotID
::
ER1
Small
;
robot
.
connect
(
id
);
// Enter ROS main loop.
...
...
src/point_processing_node/main.cpp
View file @
56486826
...
...
@@ -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.01
f
,
0.01
f
,
0.01
f
);
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
src/point_processing_node_student/main.cpp
View file @
56486826
...
...
@@ -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.01
f
,
0.01
f
,
0.01
f
);
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_processo
r"
);
ros
::
init
(
argc
,
argv
,
"crs_
tf_publishe
r"
);
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
();
...
...
src/tf_publisher_node_student/main.cpp
View file @
56486826
...
...
@@ -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
};
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment