Skip to content
Snippets Groups Projects
Commit b8fd35bb authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Update launch file

parent 0d92abca
No related branches found
No related tags found
No related merge requests found
image_width: 640
image_height: 480
camera_name: usb_cam
camera_name: head_camera
camera_matrix:
rows: 3
cols: 3
......
<!-- -->
<launch>
<!-- load robot defined machines -->
<include file="$(find tibi_dabo_base)/machines/$(env ROBOT)_$(env ROS_MODE).machines" />
<group ns="$(env ROBOT)">
<node name="local_th"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen"
machine="visio">
<param name="window_size" value="15"/>
<param name="k" value="0.25"/>
<remap from="/$(env ROBOT)/local_th/image_in/camera_info"
to="/$(env ROBOT)/sensors/head_right/camera_info"/>
<remap from="/$(env ROBOT)/local_th/image_in/image_raw"
to="/$(env ROBOT)/sensors/head_right/image_raw"/>
</node>
<node name="ar_pose"
pkg ="ar_pose"
type="ar_multi"
respawn="false"
output="screen"
machine="visio">
<param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
<param name="threshold" type="int" value="100"/>
<param name="use_history" type="bool" value="true"/>
<remap from="/$(env ROBOT)/camera/image_raw"
to="/$(env ROBOT)/local_th/image_out/image_raw"/>
<remap from="/$(env ROBOT)/camera/camera_info"
to="/$(env ROBOT)/local_th/image_out/camera_info"/>
</node>
</group>
</launch>
<!-- -->
<launch>
<!-- load robot defined machines -->
<include file="$(find tibi_dabo_base)/machines/$(env ROBOT).machines" />
<!-- tibi/dabo sensors -->
<!-- published topics: /$(env ROBOT)/sensors/front_laser_scan -->
<!-- /$(env ROBOT)/sensors/rear_laser_scan -->
<!-- /$(env ROBOT)/sensors/vertical_laser_scan -->
<!-- /$(env ROBOT)/sensors/head_right_image -->
<!-- /$(env ROBOT)/sensors/payload_battery_status -->
<!-- /$(env ROBOT)/sensors/joy -->
<!-- subscribed topics: -->
<!-- service clients: -->
<!-- service servers: -->
<!-- action clients: -->
<!-- action servers: -->
<include file="$(find tibi_dabo_base)/launch/$(env ROBOT)_sensors.launch">
<arg name="front_laser" value="False" />
<arg name="rear_laser" value="False" />
<arg name="vertical_laser" value="False" />
<arg name="wiimote" value="False" />
<arg name="bumblebee_right" value="True" />
<arg name="battery" value="False" />
</include>
<!-- tibi/dabo devices -->
<!-- published topics: $(env ROBOT)/joint_states -->
<!-- $(env ROBOT)/segway/status-->
<!-- subscribed topics: $(env ROBOT)/head/joint_position -->
<!-- $(env ROBOT)/head/joint_effort -->
<!-- $(env ROBOT)/head/facial_expression -->
<!-- $(env ROBOT)/segway/cmd_vel -->
<!-- $(env ROBOT)/joint_states -->
<!-- service clients: -->
<!-- service servers: -->
<!-- action clients: -->
<!-- action servers: $(env ROBOT)/left_arm/motion_sequence -->
<!-- $(env ROBOT)/left_arm/joint_motion -->
<!-- $(env ROBOT)/right_arm/motion_sequence -->
<!-- $(env ROBOT)/right_arm/joint_motion -->
<!-- $(env ROBOT)/head/motion_sequence -->
<!-- $(env ROBOT)/head/lights_sequence -->
<!-- $(env ROBOT)/head/joint_motion -->
<include file="$(find tibi_dabo_base)/launch/$(env ROBOT)_devices.launch">
<arg name="head" value="False" />
<arg name="left_arm" value="False" />
<arg name="right_arm" value="False" />
<arg name="platform" value="False" />
</include>
<include file="$(find iri_image_local_binarization)/launch/tibi_dabo_local_bin_nodes.launch"/>
<!-- optionally include the rviz node -->
<node name="rviz"
pkg="rviz"
type="rviz"
machine="monitor"
args="-d $(find tibi_dabo_tag_head_tracking)/config/$(env ROBOT)_head_tracking.vcg" />
</launch>
<!-- -->
<launch>
<node name="usb_cam"
pkg ="usb_cam"
type="usb_cam_node"
respawn="false"
output="screen">
<param name="video_device" type="string" value="/dev/video0"/>
<param name="camera_frame_id" type="string" value="usb_cam"/>
<param name="io_method" type="string" value="mmap"/>
<param name="image_width" type="int" value="1920"/>
<param name="image_height" type="int" value="1080"/>
<param name="framerate" type="int" value="5"/>
<param name="pixel_format" type="string" value="yuyv"/>
<param name="framerate" type="int" value="3"/>
<param name="camera_info_url" type="string" value="file://$(find iri_image_local_binarization)/calibration/usb_cam.yaml" />
<remap from="/usb_cam/camera_info"
to="/camera/camera_info"/>
<remap from="/usb_cam/image_raw"
to="/camera/image_raw"/>
</node>
<!-- published topics: /processed_image -->
<!-- subscribed topics: /camera/image_raw -->
<!-- /camera/camera_info -->
<!-- service clients: -->
<!-- service servers: -->
<!-- action clients: -->
<!-- action servers: -->
<!-- image crop -->
<node pkg ="nodelet"
type="nodelet"
name="image_proc_dec"
args="standalone image_proc/crop_decimate">
<param name="decimation_x" value="2" />
<param name="decimation_y" value="1" />
</node>
<node name="local_th"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen">
<param name="window_size" value="15"/>
<remap from="/local_th/image_in/camera_info"
to="/camera_out/camera_info"/>
<remap from="/local_th/image_in/image_raw"
to="/camera_out/image_raw"/>
</node>
<group ns="cam1">
<node name="local_th"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen">
<param name="window_size" value="15"/>
<remap from="/cam1/local_th/image_in/camera_info"
to="/camera_out/camera_info"/>
<remap from="/cam1/local_th/image_in/image_raw"
to="/camera_out/image_raw"/>
</node>
<!-- <node name="ar_pose"
pkg ="ar_pose"
type="ar_multi"
respawn="false"
output="screen">
<param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
<param name="threshold" type="int" value="100"/>
<param name="use_history" type="bool" value="true"/>
<remap from="/camera/image_raw"
to="/local_th/image_out/image_raw"/>
<remap from="/camera/camera_info"
to="/local_th/image_out/camera_info"/>
</node>-->
<node name="ar_pose"
pkg ="ar_pose"
type="ar_multi"
respawn="false"
output="screen">
<param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
<param name="threshold" type="int" value="100"/>
<param name="use_history" type="bool" value="true"/>
<remap from="/cam1/camera/image_raw"
to="/cam1/local_th/image_out/image_raw"/>
<remap from="/cam1/camera/camera_info"
to="/cam1/local_th/image_out/camera_info"/>
</node>
</group>
<group ns="cam2">
<node name="local_th"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen">
<param name="window_size" value="15"/>
<remap from="/cam2/local_th/image_in/camera_info"
to="/camera_out/camera_info"/>
<remap from="/cam2/local_th/image_in/image_raw"
to="/camera_out/image_raw"/>
</node>
<!-- <node name="image_th"
pkg="image_view"
type="image_view"
output="screen">
<remap from="/cam2/image"
to="/cam2/local_th/image_out/image_raw"/>
</node>-->
<node name="ar_pose"
pkg ="ar_pose"
type="ar_multi"
respawn="false"
output="screen">
<param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
<param name="threshold" type="int" value="100"/>
<param name="use_history" type="bool" value="true"/>
<remap from="/cam2/camera/image_raw"
to="/cam2/local_th/image_out/image_raw"/>
<remap from="/cam2/camera/camera_info"
to="/cam2/local_th/image_out/camera_info"/>
</node>
</group>
<group ns="cam3">
<node name="local_th"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen">
<param name="window_size" value="15"/>
<remap from="/cam3/local_th/image_in/camera_info"
to="/camera_out/camera_info"/>
<remap from="/cam3/local_th/image_in/image_raw"
to="/camera_out/image_raw"/>
</node>
<!-- <node name="image_th"
pkg="image_view"
type="image_view"
output="screen">
<remap from="/cam3/image"
to="/cam3/local_th/image_out/image_raw"/>
</node>-->
<node name="ar_pose"
pkg ="ar_pose"
type="ar_multi"
respawn="false"
output="screen">
<param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
<param name="threshold" type="int" value="100"/>
<param name="use_history" type="bool" value="true"/>
<remap from="/cam3/camera/image_raw"
to="/cam3/local_th/image_out/image_raw"/>
<remap from="/cam3/camera/camera_info"
to="/cam3/local_th/image_out/camera_info"/>
</node>
</group>
<group ns="cam4">
<node name="local_th"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen">
<param name="window_size" value="15"/>
<remap from="/cam4/local_th/image_in/camera_info"
to="/camera_out/camera_info"/>
<remap from="/cam4/local_th/image_in/image_raw"
to="/camera_out/image_raw"/>
</node>
<!-- <node name="image_th"
pkg="image_view"
type="image_view"
output="screen">
<remap from="/cam4/image"
to="/cam4/local_th/image_out/image_raw"/>
</node>-->
<node name="ar_pose"
pkg ="ar_pose"
type="ar_multi"
respawn="false"
output="screen">
<param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
<param name="threshold" type="int" value="100"/>
<param name="use_history" type="bool" value="true"/>
<remap from="/cam4/camera/image_raw"
to="/cam4/local_th/image_out/image_raw"/>
<remap from="/cam4/camera/camera_info"
to="/cam4/local_th/image_out/camera_info"/>
</node>
</group>
<group ns="cam5">
<node name="local_th"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen">
<param name="window_size" value="15"/>
<remap from="/cam5/local_th/image_in/camera_info"
to="/camera_out/camera_info"/>
<remap from="/cam5/local_th/image_in/image_raw"
to="/camera_out/image_raw"/>
</node>
<!-- <node name="image_th"
pkg="image_view"
type="image_view"
output="screen">
<remap from="/cam5/image"
to="/cam5/local_th/image_out/image_raw"/>
</node>-->
<node name="ar_pose"
pkg ="ar_pose"
type="ar_multi"
respawn="false"
output="screen">
<param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
<param name="threshold" type="int" value="100"/>
<param name="use_history" type="bool" value="true"/>
<remap from="/cam5/camera/image_raw"
to="/cam5/local_th/image_out/image_raw"/>
<remap from="/cam5/camera/camera_info"
to="/cam5/local_th/image_out/camera_info"/>
</node>
</group>
<group ns="cam6">
<node name="local_th"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen">
<param name="window_size" value="15"/>
<remap from="/cam6/local_th/image_in/camera_info"
to="/camera_out/camera_info"/>
<remap from="/cam6/local_th/image_in/image_raw"
to="/camera_out/image_raw"/>
</node>
<!-- <node name="image_th"
pkg="image_view"
type="image_view"
output="screen">
<remap from="/cam6/image"
to="/cam6/local_th/image_out/image_raw"/>
</node>-->
<node name="ar_pose"
pkg ="ar_pose"
type="ar_multi"
respawn="false"
output="screen">
<param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
<param name="threshold" type="int" value="100"/>
<param name="use_history" type="bool" value="true"/>
<remap from="/cam6/camera/image_raw"
to="/cam6/local_th/image_out/image_raw"/>
<remap from="/cam6/camera/camera_info"
to="/cam6/local_th/image_out/camera_info"/>
</node>
</group>
<!-- <node name="world2camera"
pkg ="tf"
type="static_transform_publisher"
args="0 0 0.0 -1.57 0 -1.57 world camera 1" />-->
<!-- <node name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find ar_pose)/config/tag.vcg"/>-->
</launch>
<!-- -->
<launch>
<arg name="crop" default="1"/>
<node name="usb_cam"
pkg ="usb_cam"
type="usb_cam_node"
respawn="false"
output="screen">
<param name="video_device" type="string" value="/dev/video0"/>
<param name="camera_frame_id" type="string" value="usb_cam"/>
<param name="io_method" type="string" value="mmap"/>
<param name="image_width" type="int" value="640"/>
<param name="image_height" type="int" value="480"/>
<param name="framerate" type="int" value="5"/>
<param name="pixel_format" type="string" value="yuyv"/>
<param name="framerate" type="int" value="3"/>
<param name="camera_info_url" type="string" value="file://$(find iri_image_local_binarization)/calibration/usb_cam_640x480.yaml" />
<remap from="/usb_cam/camera_info"
to="/camera/camera_info"/>
<remap from="/usb_cam/image_raw"
to="/camera/image_raw"/>
</node>
<!-- image crop -->
<!-- published topics: /processed_image -->
<!-- subscribed topics: /camera/image_raw -->
<!-- /camera/camera_info -->
<node pkg ="nodelet"
type="nodelet"
name="image_proc_dec"
args="standalone image_proc/crop_decimate">
<param name="decimation_x" value="$(arg crop)" />
<param name="decimation_y" value="$(arg crop)" />
</node>
<node name="image_local_binarization"
pkg="iri_image_local_binarization"
type="iri_image_local_binarization"
output="screen">
<param name="window_size" value="15"/>
<remap from="/image_local_binarization/image_in/camera_info"
to="/camera_out/camera_info"/>
<remap from="/image_local_binarization/image_in/image_raw"
to="/camera_out/image_raw"/>
</node>
<node name="rqt_image_view" pkg="rqt_image_view" type="rqt_image_view" args="/image_local_binarization/image_out/image_raw/compressed"/>
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
</launch>
......@@ -4,7 +4,7 @@ ImageLocalBinarizationAlgNode::ImageLocalBinarizationAlgNode(void) :
algorithm_base::IriBaseAlgorithm<ImageLocalBinarizationAlgorithm>(),it(this->public_node_handle_)
{
//init class attributes if necessary
this->loop_rate_ = 1;//in [Hz]
this->setRate(1);//in [Hz]
// [init publishers]
this->image_out_publisher_ = this->it.advertiseCamera("image_out/image_raw", 1);
......
......@@ -4,7 +4,7 @@ ImageLocalBinarizationAlgNodelet::ImageLocalBinarizationAlgNodelet(ros::NodeHand
algorithm_base::IriBaseAlgorithm<ImageLocalBinarizationAlgorithm>(),it(private_nh)
{
//init class attributes if necessary
this->loop_rate_ = 1;//in [Hz]
this->setRate(1);//in [Hz]
// [init publishers]
this->image_out_publisher_ = this->it.advertiseCamera("image_out/image_raw", 1);
......@@ -156,5 +156,6 @@ ImageLocalBinNodelet::~ImageLocalBinNodelet()
}
// parameters are: package, class name, class type, base class type
PLUGINLIB_DECLARE_CLASS(iri_image_local_binarization, ImageLocalBinNodelet, ImageLocalBinNodelet, nodelet::Nodelet);
//PLUGINLIB_DECLARE_CLASS(iri_image_local_binarization, ImageLocalBinNodelet, ImageLocalBinNodelet, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(ImageLocalBinNodelet, nodelet::Nodelet);
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment