diff --git a/src/tutorial_pkg/src/my_first_node.cpp b/src/tutorial_pkg/src/my_first_node.cpp index 9c60aa6..1dd6e62 100644 --- a/src/tutorial_pkg/src/my_first_node.cpp +++ b/src/tutorial_pkg/src/my_first_node.cpp @@ -1,5 +1,8 @@ #include #include +#include + +ros::Publisher brightness_pub; void imageCallback(const sensor_msgs::ImageConstPtr &image) { @@ -9,15 +12,32 @@ void imageCallback(const sensor_msgs::ImageConstPtr &image) sum += value; } int avg = sum / image->data.size(); - ROS_INFO("Brightness: %d", avg); + + std_msgs::UInt8 brightness_value; + brightness_value.data = avg; + brightness_pub.publish(brightness_value); +} + +void timerCallback(const ros::TimerEvent&) +{ + ROS_INFO("Timer activate"); + + std_srvs::Empty srv; + client.call(srv); } int main(int argc, char **argv) { - ros::init(argc, argv, "my_first_node"); - ros::NodeHandle n("~"); - ros::Subscriber sub = n.subscribe("/camera/color/image_raw", 10, imageCallback); + ros::init(argc, argv, "my_first_node"); + ros::NodeHandle n("~"); - ROS_INFO("Node started!"); - ros::spin(); -} + double timer_period_s; + n.param("timer_period_s", timer_period_s, 5.0); + + ros::Subscriber sub = n.subscribe("/camera/color/image_raw", 10, imageCallback); + brightness_pub = n.advertise("brightness", 1); + ros::Timer timer = n.createTimer(ros::Duration(timer_period_s), timerCallback); + + ROS_INFO("Node started!"); + ros::spin(); +} \ No newline at end of file