Skip to content

Commit 7cde309

Browse files
KlemenDEVKlemenDEV
committed
Some launch changes
1 parent 2a963da commit 7cde309

11 files changed

+111
-40
lines changed

‎src/gps_injection/launch/test_gps_injection.launch

+1-6
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,7 @@
1212
</node>
1313

1414
<!-- GPS injector serial messages generator and switcher -->
15-
<node pkg="gps_injection" type="gps_injection_node" name="gps"
16-
output="screen"
17-
clear_params="false"
18-
respawn="true"
19-
respawn_delay="30">
20-
</node>
15+
<node pkg="gps_injection" type="gps_injection_node" name="gps" output="screen"/>
2116

2217
<!-- Mavlink driver to listen to switch for GPS source switching -->
2318
<include file="$(find gps_injection)/launch/mavros.launch">

‎src/runs/CMakeLists.txt

+15-1
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,22 @@ project(runs)
44
find_package(catkin REQUIRED COMPONENTS
55
roscpp
66
rospy
7+
image_transport
8+
std_msgs
9+
cv_bridge
10+
sensor_msgs
711
)
812

913
catkin_package()
1014

11-
include_directories(${catkin_INCLUDE_DIRS})
15+
include_directories(${catkin_INCLUDE_DIRS})
16+
17+
# Node for monocular-inertial2 camera
18+
add_executable(color2gray
19+
src/color2gray.cc
20+
)
21+
22+
target_link_libraries(color2gray
23+
${LIBS}
24+
${catkin_LIBRARIES}
25+
)

‎src/runs/launch/bag_record_all.launch

+10-8
Original file line numberDiff line numberDiff line change
@@ -4,25 +4,27 @@
44
<!-- START SENSORS -->
55
<include file="$(find runs)/launch/subsystems/sensors.launch"/>
66

7-
<!-- REALSENSE CAMERA -->
8-
<include file="$(find runs)/launch/subsystems/rs_d435i_rgbi.launch"/>
7+
<!-- START REALSENSE CAMERA -->
8+
<include file="$(find runs)/launch/subsystems/rs_d435i_iri.launch"/>
9+
10+
<!-- START PI CAMERA MODULE -->
11+
<include file="$(find runs)/launch/subsystems/pi_camera.launch"/>
12+
13+
<!-- START GPS injector (so we can get 3D fix while flying) -->
14+
<node pkg="gps_injection" type="gps_injection_node" name="gps" output="screen"/>
915

1016
<!-- RECORD ROSBAG -->
1117
<node pkg="rosbag" type="record" name="data_capture"
1218
args="$(eval 'record --tcpnodelay --buffsize 0 -O /media/usb/rosbags/' + raw_input('Enter rec file name: ') + '.bag
1319
/camera/imu
14-
/camera/color/image_raw
15-
16-
/pi_camera/image_raw
17-
/pi_camera/camera_info
20+
/camera/infra1/image_rect_raw
21+
/camera/orthogonal
1822
1923
/ublox/fix
2024
/ublox/fix_velocity
2125
2226
/mavros/global_position/compass_hdg
2327
/mavros/global_position/rel_alt
24-
/mavros/imu/static_pressure
2528
/mavros/imu/data
2629
')"/>
27-
2830
</launch>

‎src/runs/launch/rt_matching.launch

+7
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,12 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22

33
<launch>
4+
<!-- START SENSORS -->
5+
<include file="$(find runs)/launch/subsystems/sensors.launch"/>
46

7+
<!-- GPS injector and switcher -->
8+
<node pkg="gps_injection" type="gps_injection_node" name="gps" output="screen" required="true"/>
9+
10+
<!-- MAP GPS LOCATION TO LOCAL -->
11+
<include file="$(find gps_data_fusion)/launch/gps_to_local.launch"/>
512
</launch>

‎src/runs/launch/subsystems/pi_camera.launch

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
3+
<launch>
4+
<node type="usb_cam_node" pkg="usb_cam" name="pi_camera" output="screen" required="true">
5+
<param name="video_device" value="/dev/video6"/>
6+
<param name="image_width" value="1280"/>
7+
<param name="image_height" value="720"/>
8+
<param name="pixel_format" value="yuyv" />
9+
<param name="framerate" value="2"/>
10+
<param name="focus" value="0"/>
11+
12+
<param name="camera_info_url" value="package://runs/camera_info/camerav2_1280x720.yaml"/>
13+
<param name="camera_name" value="camerav2_1280x720"/>
14+
</node>
15+
16+
<node type="color2gray" pkg="runs" name="pi_camera_grayer" output="screen" required="true">
17+
<param name="in" value="/pi_camera/image_raw"/>
18+
<param name="out" value="/camera/orthogonal"/>
19+
</node>
20+
</launch>

‎src/runs/launch/subsystems/rs_d435i_iri.launch

+2-2
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
<arg name="enable_gyro" default="true"/>
55
<arg name="enable_accel" default="true"/>
66
<arg name="enable_sync" default="true"/>
7-
<arg name="initial_reset" default="true"/> <!-- should be more reliable this way -->
7+
<arg name="initial_reset" default="true"/>
88
<arg name="unite_imu_method" default="linear_interpolation"/>
99
<arg name="infra_width" default="640"/>
1010
<arg name="infra_height" default="480"/>
@@ -13,7 +13,7 @@
1313
<arg name="infra_fps" default="15"/>
1414

1515
<rosparam>
16-
/camera/stereo_module/emitter_enabled: false
16+
/camera/stereo_module/emitter_enabled: 0
1717
</rosparam>
1818

1919
<arg name="depth_width" default="640"/>

‎src/runs/launch/subsystems/rs_d435i_rgbdi.launch

+5-1
Original file line numberDiff line numberDiff line change
@@ -8,14 +8,18 @@
88
<arg name="enable_gyro" default="true"/>
99
<arg name="enable_accel" default="true"/>
1010
<arg name="enable_sync" default="true"/>
11-
<arg name="initial_reset" default="true"/> <!-- should be more reliable this way -->
11+
<arg name="initial_reset" default="true"/>
1212
<arg name="unite_imu_method" default="linear_interpolation"/>
1313
<arg name="depth_width" default="640"/>
1414
<arg name="depth_height" default="480"/>
1515
<arg name="enable_depth" default="true"/>
1616
<arg name="depth_fps" default="15"/>
1717
<arg name="align_depth" default="true"/>
1818

19+
<rosparam>
20+
/camera/rgb_camera/auto_exposure_priority: false
21+
</rosparam>
22+
1923
<arg name="serial_no" default=""/>
2024
<arg name="json_file_path" default=""/>
2125
<arg name="camera" default="camera"/>

‎src/runs/launch/subsystems/rs_d435i_rgbi.launch

+3-2
Original file line numberDiff line numberDiff line change
@@ -8,11 +8,12 @@
88
<arg name="enable_gyro" default="true"/>
99
<arg name="enable_accel" default="true"/>
1010
<arg name="enable_sync" default="true"/>
11-
<arg name="initial_reset" default="true"/> <!-- should be more reliable this way -->
11+
<arg name="initial_reset" default="true"/>
1212
<arg name="unite_imu_method" default="linear_interpolation"/>
1313

1414
<rosparam>
15-
/camera/stereo_module/emitter_enabled: false
15+
/camera/stereo_module/emitter_enabled: 0
16+
/camera/rgb_camera/auto_exposure_priority: false
1617
</rosparam>
1718

1819
<arg name="serial_no" default=""/>

‎src/runs/launch/subsystems/sensors.launch

+6-19
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,12 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22

33
<launch>
4+
<!-- GPS MODULE -->
5+
<node pkg="ublox_gps" type="ublox_gps" name="ublox" output="screen" clear_params="false" required="true">
6+
<param name="device" type="string" value="/dev/ttyAMA4"/>
7+
<rosparam command="load" file="$(find gps_injection)/config/gps_config.yaml"/>
8+
</node>
9+
410
<!-- MAVLINK (FC SENSORS) -->
511
<include file="$(find gps_injection)/launch/mavros.launch">
612
<arg name="pluginlists_yaml" value="$(find gps_injection)/launch/apm_pluginlists.yaml"/>
@@ -16,23 +22,4 @@
1622

1723
<node pkg="rosservice" type="rosservice" name="mavros_stream_rate" args="call /mavros/set_stream_rate 0 10 1"
1824
launch-prefix="bash -c 'sleep 6; $0 $@'"/>
19-
20-
<!-- GPS MODULE -->
21-
<node pkg="ublox_gps" type="ublox_gps" name="ublox" output="screen" clear_params="false" required="true">
22-
<param name="device" type="string" value="/dev/ttyAMA4"/>
23-
<rosparam command="load" file="$(find gps_injection)/config/gps_config.yaml"/>
24-
</node>
25-
26-
<!-- RASPBERRY PI CAMERA -->
27-
<node type="usb_cam_node" pkg="usb_cam" name="pi_camera" output="screen" required="true">
28-
<param name="video_device" value="/dev/video6"/>
29-
<param name="image_width" value="1280"/>
30-
<param name="image_height" value="720"/>
31-
<param name="pixel_format" value="yuyv" />
32-
<param name="framerate" value="2"/>
33-
<param name="focus" value="0"/>
34-
35-
<param name="camera_info_url" value="package://runs/camera_info/camerav2_1280x720.yaml"/>
36-
<param name="camera_name" value="camerav2_1280x720"/>
37-
</node>
3825
</launch>

‎src/runs/package.xml

+6-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,12 @@
1515
<build_export_depend>rospy</build_export_depend>
1616
<exec_depend>roscpp</exec_depend>
1717
<exec_depend>rospy</exec_depend>
18-
18+
19+
<depend>sensor_msgs</depend>
20+
<depend>cv_bridge</depend>
21+
<depend>std_msgs</depend>
22+
<depend>image_transport</depend>
23+
1924
<export>
2025
</export>
2126
</package>

‎src/runs/src/color2gray.cc

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
#include <ros/ros.h>
2+
#include <image_transport/image_transport.h>
3+
#include <cv_bridge/cv_bridge.h>
4+
#include <sensor_msgs/image_encodings.h>
5+
6+
image_transport::Publisher image_pub;
7+
8+
void imageCb(const sensor_msgs::ImageConstPtr &msg) {
9+
cv_bridge::CvImageConstPtr cv_ptr;
10+
try {
11+
cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8);
12+
} catch (cv_bridge::Exception &e) {
13+
ROS_ERROR("cv_bridge exception: %s", e.what());
14+
return;
15+
}
16+
image_pub.publish(cv_ptr->toImageMsg());
17+
}
18+
19+
int main(int argc, char **argv) {
20+
ros::init(argc, argv, "color2gray");
21+
ros::NodeHandle n;
22+
ros::NodeHandle nh("~");
23+
24+
std::string topic_in;
25+
nh.getParam("in", topic_in);
26+
std::string topic_out;
27+
nh.getParam("out", topic_out);
28+
29+
image_transport::ImageTransport it(n);
30+
image_transport::Subscriber image_sub = it.subscribe(topic_in, 1, imageCb);
31+
image_pub = it.advertise(topic_out, 1);
32+
33+
ros::spin();
34+
35+
return 0;
36+
}

0 commit comments

Comments
 (0)