Skip to content

Commit bb17c65

Browse files
Alexey-KamenevAlexey-Kamenev
authored
Merge pull request #31 from NVIDIA-Jetson/alexeyk/scratch
Improved RViz-based debugging support.
2 parents ab401ac + e495d08 commit bb17c65

File tree

15 files changed

+404
-24
lines changed

15 files changed

+404
-24
lines changed

‎ros/packages/caffe_ros/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,4 +21,4 @@
2121

2222
<export>
2323
</export>
24-
</package>
24+
</package>

‎ros/packages/caffe_ros/src/caffe_ros.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -112,9 +112,10 @@ sensor_msgs::Image::ConstPtr CaffeRos::computeOutputs()
112112
auto img = *cur_img_;
113113
net_.forward(img.data.data(), img.width, img.height, 3);
114114
auto out_msg = boost::make_shared<sensor_msgs::Image>();
115-
// Set stamp to the same value as source image so we can synchronize with other nodes if needed.
115+
// Set stamp and frame id to the same value as source image so we can synchronize with other nodes if needed.
116116
out_msg->header.stamp.sec = img.header.stamp.sec;
117117
out_msg->header.stamp.nsec = img.header.stamp.nsec;
118+
out_msg->header.frame_id = img.header.frame_id;
118119

119120
// Use single precision multidimensional array to represent outputs.
120121
// This can be useful in case DNN output is multidimensional such as in segmentation networks.

‎ros/packages/image_pub/CMakeLists.txt

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,14 @@ project(image_pub)
44
find_package(catkin REQUIRED COMPONENTS
55
roscpp
66
std_msgs
7+
sensor_msgs
8+
camera_info_manager
9+
)
10+
find_package(OpenCV REQUIRED)
11+
12+
catkin_package(
13+
CATKIN_DEPENDS roscpp std_msgs sensor_msgs camera_info_manager
14+
DEPENDS OpenCV
715
)
816

917
###########
@@ -14,7 +22,6 @@ set (CMAKE_CXX_STANDARD 14)
1422
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
1523

1624
include_directories(
17-
include
1825
${catkin_INCLUDE_DIRS}
1926
)
2027

‎ros/packages/image_pub/package.xml

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,10 +11,16 @@
1111

1212
<build_depend>roscpp</build_depend>
1313
<build_depend>std_msgs</build_depend>
14+
<build_depend>sensor_msgs</build_depend>
15+
<build_depend>camera_info_manager</build_depend>
16+
<build_depend>opencv</build_depend>
1417

1518
<run_depend>roscpp</run_depend>
1619
<run_depend>std_msgs</run_depend>
20+
<run_depend>sensor_msgs</run_depend>
21+
<run_depend>camera_info_manager</run_depend>
22+
<run_depend>opencv</run_depend>
1723

1824
<export>
1925
</export>
20-
</package>
26+
</package>

‎ros/packages/image_pub/src/image_pub_node.cpp

Lines changed: 35 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,12 @@
22
// Full license terms provided in LICENSE.md file.
33

44
#include <ros/ros.h>
5+
6+
#include <camera_info_manager/camera_info_manager.h>
7+
58
#include <sensor_msgs/Image.h>
9+
#include <sensor_msgs/CameraInfo.h>
10+
611
#include <opencv2/opencv.hpp>
712

813
int main(int argc, char **argv)
@@ -13,23 +18,37 @@ int main(int argc, char **argv)
1318
ros::NodeHandle nh("~");
1419

1520
std::string camera_topic;
21+
std::string camera_info_topic;
22+
std::string camera_info_url;
1623
std::string img_path;
24+
std::string frame_id;
1725
float pub_rate;
1826
int start_sec;
1927
bool repeat;
20-
nh.param<std::string>("camera_topic", camera_topic, "/camera/image_raw");
28+
nh.param<std::string>("camera_topic", camera_topic, "/camera/image_raw");
29+
nh.param<std::string>("camera_info_topic", camera_info_topic, "/camera/camera_info");
30+
nh.param<std::string>("camera_info_url", camera_info_url, "");
2131
nh.param<std::string>("img_path", img_path, "");
32+
nh.param<std::string>("frame_id", frame_id, "");
2233
nh.param("pub_rate", pub_rate, 30.0f);
2334
nh.param("start_sec", start_sec, 0);
2435
nh.param("repeat", repeat, false);
2536

26-
ROS_INFO("Topic : %s", camera_topic.c_str());
27-
ROS_INFO("Source: %s", img_path.c_str());
28-
ROS_INFO("Rate : %.1f", pub_rate);
29-
ROS_INFO("Start : %d", start_sec);
30-
ROS_INFO("Repeat: %s", repeat ? "yes" : "no");
37+
ROS_INFO("CTopic : %s", camera_topic.c_str());
38+
ROS_INFO("ITopic : %s", camera_info_topic.c_str());
39+
ROS_INFO("CI URL : %s", camera_info_url.c_str());
40+
ROS_INFO("Source : %s", img_path.c_str());
41+
ROS_INFO("Rate : %.1f", pub_rate);
42+
ROS_INFO("Start : %d", start_sec);
43+
ROS_INFO("Repeat : %s", repeat ? "yes" : "no");
44+
ROS_INFO("FrameID: %s", frame_id.c_str());
45+
46+
camera_info_manager::CameraInfoManager camera_info_manager(nh);
47+
if (camera_info_manager.validateURL(camera_info_url))
48+
camera_info_manager.loadCameraInfo(camera_info_url);
3149

32-
ros::Publisher img_pub = nh.advertise<sensor_msgs::Image>(camera_topic, 100);
50+
ros::Publisher img_pub = nh.advertise<sensor_msgs::Image>(camera_topic, 1);
51+
ros::Publisher info_pub = nh.advertise<sensor_msgs::CameraInfo>(camera_info_topic, 1);
3352

3453
cv::VideoCapture vid_cap(img_path.c_str());
3554
if (start_sec > 0)
@@ -60,14 +79,22 @@ int main(int argc, char **argv)
6079
cv::cvtColor(img, img, CV_BGR2RGB);
6180

6281
auto img_msg = boost::make_shared<sensor_msgs::Image>();
63-
img_msg->header.stamp = ros::Time::now();
82+
img_msg->header.stamp = ros::Time::now();
83+
img_msg->header.frame_id = frame_id;
6484
img_msg->encoding = "rgb8";
6585
img_msg->width = img.cols;
6686
img_msg->height = img.rows;
6787
img_msg->step = img_msg->width * img.channels();
6888
auto ptr = img.ptr<unsigned char>(0);
6989
img_msg->data = std::vector<unsigned char>(ptr, ptr + img_msg->step * img_msg->height);
7090
img_pub.publish(img_msg);
91+
92+
if (camera_info_manager.isCalibrated())
93+
{
94+
auto info = boost::make_shared<sensor_msgs::CameraInfo>(camera_info_manager.getCameraInfo());
95+
info->header = img_msg->header;
96+
info_pub.publish(info);
97+
}
7198
}
7299
ros::spinOnce();
73100
rate.sleep();

‎ros/packages/px4_controller/launch/aion_ctl.launch

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,12 @@
44
<arg name="linear_speed" default="2.0" />
55
<arg name="linear_speed_scale" default="90" />
66
<arg name="turn_angle_scale" default="-250" />
7-
<arg name="dnn_turn_angle" default="45.0" />
8-
<arg name="dnn_lateralcorr_angle" default="45.0" />
7+
<arg name="dnn_turn_angle" default="30.0" />
8+
<arg name="dnn_lateralcorr_angle" default="30.0" />
99
<arg name="joy_type" default="shield" />
10+
<arg name="log_output" default="screen" />
1011

11-
<node pkg="px4_controller" type="px4_controller_node" name="px4_controller">
12+
<node pkg="px4_controller" type="px4_controller_node" name="px4_controller" output="$(arg log_output)">
1213
<param name="vehicle_type" value="$(arg vehicle_type)" />
1314
<param name="altitude_gain" value="$(arg altitude_gain)" />
1415
<param name="linear_speed" value="$(arg linear_speed)" />
@@ -19,4 +20,3 @@
1920
<param name="joy_type" value="$(arg joy_type)" />
2021
</node>
2122
</launch>
22-

‎ros/packages/px4_controller/launch/nomad_ctl.launch

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,9 @@
77
<arg name="dnn_turn_angle" default="15.0" />
88
<arg name="dnn_lateralcorr_angle" default="15.0" />
99
<arg name="joy_type" default="shield" />
10+
<arg name="log_output" default="screen" />
1011

11-
<node pkg="px4_controller" type="px4_controller_node" name="px4_controller">
12+
<node pkg="px4_controller" type="px4_controller_node" name="px4_controller" output="$(arg log_output)">
1213
<param name="vehicle_type" value="$(arg vehicle_type)" />
1314
<param name="altitude_gain" value="$(arg altitude_gain)" />
1415
<param name="linear_speed" value="$(arg linear_speed)" />
@@ -19,4 +20,3 @@
1920
<param name="joy_type" value="$(arg joy_type)" />
2021
</node>
2122
</launch>
22-

‎ros/packages/px4_controller/package.xml

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,4 +57,3 @@
5757

5858
</export>
5959
</package>
60-

‎ros/packages/redtail_debug/CMakeLists.txt

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
cmake_minimum_required(VERSION 3.1)
2+
project(redtail_debug)
3+
4+
find_package(catkin REQUIRED COMPONENTS
5+
roscpp
6+
std_msgs
7+
geometry_msgs
8+
cmake_modules
9+
tf
10+
eigen_conversions
11+
)
12+
find_package(Eigen3 REQUIRED)
13+
14+
catkin_package(
15+
CATKIN_DEPENDS
16+
roscpp
17+
std_msgs
18+
geometry_msgs
19+
cmake_modules
20+
tf
21+
eigen_conversions
22+
DEPENDS EIGEN3
23+
)
24+
25+
###########
26+
## Build ##
27+
###########
28+
29+
set (CMAKE_CXX_STANDARD 14)
30+
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
31+
32+
include_directories(
33+
${catkin_INCLUDE_DIRS}
34+
)
35+
36+
file(GLOB redtail_debug_sources src/*.cpp)
37+
38+
add_executable(redtail_debug_node ${redtail_debug_sources})
39+
40+
target_link_libraries(redtail_debug_node
41+
${catkin_LIBRARIES}
42+
${tf_LIBRARIES}
43+
)

‎ros/packages/redtail_debug/launch/trailnet_debug.launch

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
<launch>
2+
<arg name="prototxt_path" default="/home/nvidia/redtail/models/pretrained/TrailNet_SResNet-18.prototxt"/>
3+
<arg name="model_path" default="/home/nvidia/redtail/models/pretrained/TrailNet_SResNet-18.caffemodel" />
4+
<arg name="input_layer" default="data" />
5+
<arg name="output_layer" default="out" />
6+
<arg name="use_fp16" default="true" />
7+
8+
<arg name="frame_id" default="/tf_frame" />
9+
10+
<arg name="device" default="/dev/video1" />
11+
12+
<!-- Defaults are for ZED WVGA -->
13+
<arg name="img_width" default="1344" />
14+
<arg name="img_height" default="376" />
15+
<arg name="camera_info_url" default="file:///home/nvidia/zed_wvga.yaml" />
16+
<!-- Use left ZED camera -->
17+
<arg name="right_crop" default="$(eval int(0.5 * arg('img_width')))" />
18+
19+
<arg name="host_ip" default="10.42.0.211" />
20+
<!-- Start the GSCAM node -->
21+
<node pkg="gscam" type="gscam" name="gscam" output="screen">
22+
<!-- Use 2 sinks (UDP H.265 streaming + ROS topic) -->
23+
<param name="gscam_config" value="v4l2src device=$(arg device) ! video/x-raw, width=$(arg img_width), height=$(arg img_height) ! videocrop top=0 left=0 right=$(arg right_crop) bottom=0 ! tee name=t ! queue ! videoconvert ! omxh265enc ! video/x-h265, stream-format=byte-stream ! h265parse ! rtph265pay config-interval=1 ! udpsink host=$(arg host_ip) port=6000 t. ! queue ! videoconvert" />
24+
<param name="camera_info_url" value="$(arg camera_info_url)" />
25+
<param name="frame_id" value="$(arg frame_id)" />
26+
</node>
27+
28+
<!-- Start the caffe_ros node -->
29+
<node pkg="caffe_ros" type="caffe_ros_node" name="trails_dnn" output="screen">
30+
<param name="prototxt_path" value="$(arg prototxt_path)" />
31+
<param name="model_path" value="$(arg model_path)" />
32+
<param name="input_layer" value="$(arg input_layer)" />
33+
<param name="output_layer" value="$(arg output_layer)" />
34+
<param name="use_fp16" value="$(arg use_fp16)" />
35+
</node>
36+
37+
<node pkg="redtail_debug" type="redtail_debug_node" name="redtail_debug" output="screen">
38+
</node>
39+
40+
<node name="tf_publisher" pkg="tf" type="static_transform_publisher" args="0 0 0 3.14 0 0 /map $(arg frame_id) 100"/>
41+
</launch>

0 commit comments

Comments
 (0)