Skip to content

Commit

Permalink
Code now compiles and executes on Verdino
Browse files Browse the repository at this point in the history
  • Loading branch information
Verdino committed Apr 29, 2015
1 parent 7d511e6 commit c9da5f6
Show file tree
Hide file tree
Showing 10 changed files with 57 additions and 232 deletions.
221 changes: 23 additions & 198 deletions launch/VoxelGridTrackingVerdino.launch
View file
Original file line number Diff line number Diff line change
@@ -1,207 +1,32 @@
<launch>
<arg name="namespace" default="verdino"/>
<arg name="test_name" default="verdino"/>
<arg name="use_ps4" default="false"/>
<arg name="use_elas" default="false"/>
<arg name="use_rsgm" default="false"/>
<arg name="use_velodyne" default="true"/>
<arg name="env_name" default="verdino"/>
<arg name="show_rviz" default="true"/>
<arg name="do_publish_intermediate_info" value="false" />

<arg name="voxel_tracking_params_file"
default="$(find voxel_grid_tracking)/params/voxel_tracking_verdino_params.yaml"/>
<arg name="disparity_filters_params_file"
default="$(find disparity_filters)/params/disparity_filters_params.yaml"/>

<arg name="use_bag" default="true"/>
<arg name="bag_name" default="/local/imaged/research_data/bags/pruebasEyefish2.bag"/>

<group if="$(arg use_bag)" >

<param name="use_sim_time" value="true"/>

<!-- <node pkg="tf" type="static_transform_publisher" name="map2odom" args="0 0 0 0 0 0 map odom 10" /> -->
<!-- <node pkg="tf" type="static_transform_publisher" name="odom2basefootprint" args="0 0 0 0 0 0 odom base_footprint 10" /> -->
</group>
<!-- else -->
<group unless="$(arg use_bag)" >
<include file="$(find grull_verdino)/launch/navigation/parking_navigation.launch">
<arg name="namespace" value="verdino"/>
<arg name="use_gps" value="false"/>
<arg name="use_filters" value="true"/>
<arg name="simulation" value="false"/>
<arg name="velodyne" value="true"/>
<arg name="show_rviz" value="false"/>
<arg name="show_rqt" value="false"/>
<arg name="show_console" value="false"/>
<arg name="world_name" value="$(find grull_gazebo_models)/worlds/parking_etsii.world"/>

<!-- Params File Names -->
<arg name="costmap_local"
value="$(find voxel_grid_tracking)/params/local_costmap_params_verdino.yaml"/>

</include>
</group>

<group if="$(arg use_ps4)">
<include file="$(find grull_stereo)/launch/stereo.launch" >
<arg name="DEVICE_LEFT" value="/dev/video1"/>
<arg name="DEVICE_RIGHT" value="/dev/video0"/>
<arg name="FRAME_NAME" value="stereo_ps4"/>
</include>

<remap from="image_rect_color" to="image_rect_eyefish" />

</group>

<group ns="$(arg namespace)">
<group if="$(arg use_elas)">
<node pkg="grull_elas_ros" type="elas_ros" name="elas_ros" output="screen">
<remap from="stereo/left/$(arg namespace)/image" to="/stereo/left/image_rect_color/compressed" />
<remap from="stereo/right/$(arg namespace)/image" to="/stereo/right/image_rect_color/compressed" />
<remap from="stereo/left/camera_info" to="/stereo/left/camera_info" />
<remap from="stereo/right/camera_info" to="/stereo/right/camera_info" />

<param name="approximate_sync" value="true" />

<!-- <remap from="base_frame_id" to="left_cam" /> -->
<param name="base_frame_id" value="left_cam" />
<param name="pose_frame_id" value="base_link" />
</node>
</group>

<group if="$(arg use_rsgm)">
<node pkg="rsgm_ros" type="rsgm_ros" name="rsgm_ros" output="screen" required="false">
<remap if="$(arg use_bag)" from="stereo/left/$(arg namespace)/image" to="/stereo/left/image_rect_color/compressed" />
<remap if="$(arg use_bag)" from="stereo/right/$(arg namespace)/image" to="/stereo/right/image_rect_color/compressed" />
<remap unless="$(arg use_bag)" from="stereo/left/$(arg namespace)/image" to="/stereo/left/image_rect_color" />
<remap unless="$(arg use_bag)" from="stereo/right/$(arg namespace)/image" to="/stereo/right/image_rect_color" />
<remap from="stereo/left/camera_info" to="/stereo/left/camera_info" />
<remap from="stereo/right/camera_info" to="/stereo/right/camera_info" />

<param name="approximate_sync" value="true" />

<remap from="~depth" to="/$(arg namespace)/depth" />
<remap from="~disparity" to="/$(arg namespace)/disparity" />

<rosparam file="$(find rsgm_ros)/params/rsgm_ros_params_verdino.yaml" command="load" ns="" />
</node>
</group>

<!-- <node launch-prefix="gdb &#45;&#45;args" name="voxel_grid_tracking" pkg="voxel_grid_tracking" type="voxel_grid_tracking" output="screen" required="true" > -->
<!-- <node launch-prefix="gdb -ex run &#45;&#45;args" name="voxel_grid_tracking" pkg="voxel_grid_tracking" type="voxel_grid_tracking" output="screen" required="true" > -->
<node name="voxel_grid_tracking" pkg="voxel_grid_tracking" type="voxel_grid_tracking" output="screen" required="true" >
<remap from="~/deltaTime"
to="/$(arg namespace)/stereo_and_odom/deltaTime" />
<remap from="~/pointCloud"
to="/$(arg namespace)/obstaclesPointCloud" />
<remap from="~/flow_vectors"
to="/$(arg namespace)/flow_vectors" />
<remap from="~/left/camera_info"
to="/$(arg namespace)/stereo_and_odom/left/camera_info" />
<remap from="~/right/camera_info"
to="/$(arg namespace)/stereo_and_odom/right/camera_info" />
<remap from="~/dbg/image_rect_color"
to="/$(arg namespace)/stereo_and_odom/left/image_rect_color" />
<remap from="~fakePointCloud"
to="/verdino/fakePointCloud" />

<rosparam file="$(arg voxel_tracking_params_file)" command="load" ns="" />

<param name="publish_intermediate_info" value="$(arg do_publish_intermediate_info)" />
</node>
</group>

<!-- I must place the remap here. For some reason, it is ignored in other location -->
<!-- <remap from="/filters/filter_manager/disparity_input" to="/$(arg namespace)/elas_ros/disparity" /> -->
<remap from="/filters/filter_manager/disparity_input" to="/$(arg namespace)/disparity" />
<remap from="/filters/filter_manager/left/camera_info" to="/$(arg namespace)/stereo_and_odom/left/camera_info" />
<remap from="/filters/filter_manager/right/camera_info" to="/$(arg namespace)/stereo_and_odom/right/camera_info" />
<group ns="filters">
<group unless="$(arg use_velodyne)" >
<!-- Filter Manager -->
<node pkg="nodelet" type="nodelet" name="filter_manager" args="manager" output="screen" required="false" />

<rosparam file="$(arg disparity_filters_params_file)" command="load" ns="DisparityFiltersNodelet" />
<node pkg="nodelet" type="nodelet" name="DisparityFiltersNodelet"
args="load disparity_filters/DisparityFiltersNodelet filter_manager" output="screen">
<param name="threads" value="4" />
<param name="disp_count" value="128" />
<param name="approximate_sync" value="true" />
<param name="queue_size" value="10" />

<param name="despeckle_filter" value="true" />
<param name="min_speckle_segment_size" value="100" />
<param name="speckle_sim_threshold" value="1.0" />

<param name="adaptive_mean_filter" value="true" />
<param name="gap_filter" value="true" />
<param name="block_filter" value="true" />
<param name="block_width" value="20" />
<param name="block_height" value="20" />
</node>

<node pkg="nodelet" type="nodelet" name="Depth2PointCloudNodelet"
args="load depth_image_proc/point_cloud_xyzrgb filter_manager" output="screen">

<remap from="rgb/image_rect_color" to="/$(arg namespace)/stereo_and_odom/left/image_rect_color" />
<remap from="rgb/camera_info" to="/$(arg namespace)/stereo_and_odom/left/camera_info" />
<!-- <remap from="depth_registered/image_rect" to="/$(arg namespace)/depth" /> -->
<remap from="depth_registered/image_rect" to="/filters/filter_manager/depth" />
</node>
</group>

<!-- PCL Manager -->
<node pkg="nodelet" type="nodelet" name="box_filter_manager" args="manager" output="screen" required="true" />

<include file="$(find grull_transform_pointcloud)/launch/launch.launch">
<arg name="nodelet_name" value="cam2footprint_pc_transform" />
<!-- <arg name="input_topic" value="/filters/depth_registered/points" /> -->
<arg if="$(arg use_elas)" name="input_topic" value="/$(arg namespace)/elas_ros/point_cloud" />
<arg if="$(arg use_velodyne)" name="input_topic" value="/verdino/velodyne_points" />
<arg name="output_topic" value="/$(arg namespace)/point_cloud_footprint"/>
<arg name="target_frame" value="base_footprint"/>
<arg name="nodelet_manager" value="box_filter_manager"/>
</include>

<!-- Run a passthrough filter to delimit in x direction -->
<node pkg="nodelet" type="nodelet" name="psx" args="load pcl/PassThrough box_filter_manager" output="screen" required="true">
<remap from="~input" to="/$(arg namespace)/point_cloud_footprint" />
<param name="filter_field_name" value="x" />
</node>

<!-- Run a passthrough filter to delimit in y direction -->
<node pkg="nodelet" type="nodelet" name="psy" args="load pcl/PassThrough box_filter_manager" output="screen" required="true">
<remap from="~input" to="psx/output" />
<param name="filter_field_name" value="y" />
</node>

<!-- Run a passthrough filter to delimit in z direction -->
<node pkg="nodelet" type="nodelet" name="psz" args="load pcl/PassThrough box_filter_manager" output="screen" required="true">
<remap from="~input" to="psy/output" />
<param name="filter_field_name" value="z" />
</node>

<rosparam file="$(find voxel_grid_tracking)/params/$(arg test_name).yaml" command="load" ns="" />

<include file="$(find grull_transform_pointcloud)/launch/launch.launch">
<arg name="nodelet_name" value="footprint2map_pc_transform" />
<arg name="input_topic" value="psz/output" />
<arg name="output_topic" value="/$(arg namespace)/obstaclesPointCloud"/>
<arg name="target_frame" value="map"/>
<arg name="nodelet_manager" value="box_filter_manager"/>
</include>
</group>

<group if="$(arg show_rviz)">
<!-- <node launch-prefix="optirun" pkg="rviz" type="rviz" name="rviz" required="false" args="&#45;&#45;display-config -->
<node pkg="rviz" type="rviz" name="rviz" required="false" args="&#45;&#45;display-config
$(find voxel_grid_tracking)/config/VoxelGridTrackingVerdino.rviz"/>
<!--<node pkg="rosbag" type="record" name="rosbag"
args="-o /tmp/2011_09_26_drive_0091_sync_" />-->
</group>
<include file="$(find grull_verdino)/launch/navigation/parking_navigation.launch">
<arg name="namespace" value="verdino"/>
<arg name="use_gps" value="false"/>
<arg name="use_filters" value="true"/>
<arg name="simulation" value="false"/>
<arg name="velodyne" value="true"/>
<arg name="show_rviz" value="false"/>
<arg name="show_rqt" value="false"/>
<arg name="show_console" value="false"/>
<arg name="world_name" value="$(find grull_gazebo_models)/worlds/parking_etsii.world"/>

<!-- Params File Names -->
<arg name="costmap_local"
value="$(find voxel_grid_tracking)/params/local_costmap_params_verdino.yaml"/>
</include>

<node if="$(arg use_bag)" name="play" pkg="rosbag" type="play" required="true"
args=" -r 1.0 -s 10 &#45;&#45;clock $(arg bag_name)" />
<!-- args=" -r 0.2 -s 20 &#45;&#45;clock $(arg bag_name)" /> -->
<include file="$(find voxel_grid_tracking)/launch/launch.launch">
<arg name="namespace" value="$(arg namespace)"/>
<arg name="env_name" value="$(arg namespace)"/>
<arg name="show_rviz" value="$(arg show_rviz)"/>
<arg name="input_topic" value="/$(arg namespace)/velodyne_points"/>
<arg name="voxel_tracking_params_file" value="/$(arg voxel_tracking_params_file)"/>
</include>
</launch>
4 changes: 2 additions & 2 deletions params/verdino.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
psx:
filter_limit_min: 0.0
filter_limit_max: 20.0
filter_limit_min: -10.0
filter_limit_max: 10.0

psy:
filter_limit_min: -4.0
Expand Down
2 changes: 1 addition & 1 deletion src/ObstaclesFromStereo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -794,7 +794,7 @@ void ObstaclesFromStereo::readEgoValues(const std::string & pathName, vector< t_
egoValues[0].deltaPos = 0;
}

vector< polar_grid_tracking::roiArray > ObstaclesFromStereo::readROIList(const string & trackletsPath, const uint32_t & sequenceLength)
vector< voxel_grid_tracking::roiArray > ObstaclesFromStereo::readROIList(const string & trackletsPath, const uint32_t & sequenceLength)
{
// cout << "trackletsPath " << trackletsPath << endl;
//
Expand Down
4 changes: 2 additions & 2 deletions src/ObstaclesFromStereo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

#include "params_structs.h"

#include "polar_grid_tracking/roiArray.h"
#include "voxel_grid_tracking/roiArray.h"
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

Expand Down Expand Up @@ -72,7 +72,7 @@ class ObstaclesFromStereo {

static void showCameraParams(const t_Camera_params & params);

static vector <polar_grid_tracking::roiArray> readROIList(const string & trackletsPath, const uint32_t & sequenceLength);
static vector <voxel_grid_tracking::roiArray> readROIList(const string & trackletsPath, const uint32_t & sequenceLength);
static vector< visualization_msgs::MarkerArray > readMarkerList(const string & trackletsPath, const uint32_t & sequenceLength);
private:
void setParamsGeometry(t_Camera_params & params);
Expand Down
16 changes: 8 additions & 8 deletions src/polargridtrackingros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,8 @@
// #include "tf2_ros/buffer.h"

#include "utilspolargridtracking.h"
#include "polar_grid_tracking/roiArray.h"
#include "polar_grid_tracking/voxel_tracker_time_stats.h"
#include "voxel_grid_tracking/roiArray.h"
#include "voxel_grid_tracking/voxel_tracker_time_stats.h"

// #include "utils.h"

Expand Down Expand Up @@ -126,8 +126,8 @@ polar_grid_trackingROS::polar_grid_trackingROS(const uint32_t& rows, const uint3
m_obstaclesPub = nh.advertise<visualization_msgs::MarkerArray>("obstacles", 10);
m_roiPub = nh.advertise<visualization_msgs::MarkerArray>("obstaclesROI", 10);
m_pointCloudInObstaclePub = nh.advertise<sensor_msgs::PointCloud2> ("pointCloudInObstacle", 1);
m_ROIPub = nh.advertise<roiArray>("result_rois", 1);
m_timeStatsPub = nh.advertise<voxel_tracker_time_stats>("time_stats", 1);
m_ROIPub = nh.advertise<voxel_grid_tracking::roiArray>("result_rois", 1);
m_timeStatsPub = nh.advertise<voxel_grid_tracking::voxel_tracker_time_stats>("time_stats", 1);

m_lastMapOdomTransform.stamp_ = ros::Time(-1);
}
Expand Down Expand Up @@ -373,7 +373,7 @@ void polar_grid_trackingROS::pointCloudCallback(const sensor_msgs::PointCloud2::

void polar_grid_trackingROS::compute(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr& pointCloud)
{
voxel_tracker_time_stats timeStatsMsg;
voxel_grid_tracking::voxel_tracker_time_stats timeStatsMsg;
timeStatsMsg.header.seq = m_currentId;
timeStatsMsg.header.stamp = ros::Time::now();

Expand Down Expand Up @@ -1060,7 +1060,7 @@ void polar_grid_trackingROS::publishROIs()

void polar_grid_trackingROS::publishRoiArrays()
{
roiArray roiMsg;
voxel_grid_tracking::roiArray roiMsg;
roiMsg.rois3d.reserve(m_obstacles.size());
roiMsg.rois2d.reserve(m_obstacles.size());

Expand All @@ -1087,8 +1087,8 @@ void polar_grid_trackingROS::publishRoiArrays()
const pcl::PointCloud<pcl::PointXYZ>::Ptr & roi = m_obstacles[i].roi();

vector<geometry_msgs::Point> points(8);
roi_and_speed_2d roi2D;
roi_and_speed_3d roi3D;
voxel_grid_tracking::roi_and_speed_2d roi2D;
voxel_grid_tracking::roi_and_speed_3d roi3D;

for (uint32_t j = 0; j < 8; j++) {
points[j].x = roi->at(j).x;
Expand Down
4 changes: 2 additions & 2 deletions src/utilspolargridtracking.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,8 +146,8 @@ geometry_msgs::Point32 toPoint32(const pcl::PointXYZRGB & point) {

return newPoint;
}
polar_grid_tracking::point_2d toPoint2D(const pcl::PointXYZRGB & point) {
polar_grid_tracking::point_2d newPoint;
voxel_grid_tracking::point_2d toPoint2D(const pcl::PointXYZRGB & point) {
voxel_grid_tracking::point_2d newPoint;
newPoint.u = point.x;
newPoint.v = point.y;

Expand Down
4 changes: 2 additions & 2 deletions src/utilspolargridtracking.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@
#include <tf/transform_datatypes.h>

#include <geometry_msgs/Point32.h>
#include <polar_grid_tracking/point_2d.h>
#include <voxel_grid_tracking/point_2d.h>

using namespace std;

Expand Down Expand Up @@ -62,7 +62,7 @@ void project3dTo2d(const pcl::PointXYZRGB & point3d, pcl::PointXYZRGB & point2d,
void project3dTo2d(const pcl::PointXYZRGB & point3d, pcl::PointXYZRGB & point2d,
const image_geometry::StereoCameraModel & stereoCameraModel);
geometry_msgs::Point32 toPoint32(const pcl::PointXYZRGB & point);
polar_grid_tracking::point_2d toPoint2D(const pcl::PointXYZRGB & point);
voxel_grid_tracking::point_2d toPoint2D(const pcl::PointXYZRGB & point);

// From http://stackoverflow.com/questions/16451111/cvmat-conversion-to-eigen-matrix-and-back
template<typename _Tp, int _rows, int _cols, int _options, int _maxRows, int _maxCols>
Expand Down
Loading

0 comments on commit c9da5f6

Please sign in to comment.