This commit is contained in:
parent
c5c0dcf2d5
commit
25fe0091e6
|
@ -1,5 +1,18 @@
|
|||
<launch>
|
||||
|
||||
<arg name="use_gazebo" default="false"/>
|
||||
|
||||
<include unless="$(arg use_gazebo)" file="$(find astra_camera)/launch/astra.launch"/>
|
||||
<include if="$(arg use_gazebo)" file="$(find rosbot_bringup)/launch/rosbot_tutorial.launch"/>
|
||||
|
||||
<node pkg="tutorial_pkg" type="my_first_node" name="my_first_node" output="screen"/>
|
||||
<param name="timer_period_s" value="2"/>
|
||||
</node>
|
||||
|
||||
<node pkg="image_view" type="image_saver" name="image_saver" output="screen">
|
||||
<remap from="/image" to="/camera/color/image_raw"/>
|
||||
<remap from="/camera_info" to="/camera/color/camera_info"/>
|
||||
<param name="save_all_image" value="false" />
|
||||
</node>
|
||||
|
||||
</launch>
|
|
@ -2,6 +2,9 @@
|
|||
#include <sensor_msgs/Image.h>
|
||||
#include <std_msgs/UInt8.h>
|
||||
#include <std_srvs/Empty.h>
|
||||
#include <std_srvs/Trigger.h>
|
||||
|
||||
uint saved_imgs = 0;
|
||||
|
||||
ros::Publisher brightness_pub;
|
||||
ros::ServiceClient client;
|
||||
|
@ -26,6 +29,14 @@ void timerCallback(const ros::TimerEvent&)
|
|||
|
||||
std_srvs::Empty srv;
|
||||
client.call(srv);
|
||||
saved_imgs++;
|
||||
}
|
||||
|
||||
bool saved_img(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
|
||||
{
|
||||
res.success = 1;
|
||||
res.message = "Saved images: " + std::to_string(saved_imgs);
|
||||
return true;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
|
@ -40,6 +51,7 @@ int main(int argc, char **argv)
|
|||
brightness_pub = n.advertise<std_msgs::UInt8>("brightness", 1);
|
||||
ros::Timer timer = n.createTimer(ros::Duration(timer_period_s), timerCallback);
|
||||
client = n.serviceClient<std_srvs::Empty>("/image_saver/save");
|
||||
ros::ServiceServer service = n.advertiseService("image_counter", saved_img);
|
||||
|
||||
ROS_INFO("Node started!");
|
||||
ros::spin();
|
||||
|
|
Loading…
Reference in New Issue