How to make a subscriber and publisher in ROS on C++ in one file? I tried this and publisher works but subscriber callback function is not called
#include <ros/ros.h>
#include <iostream>
#include <std_msgs/UInt16.h>
#include <math.h>
int error = 0;
void error_sub(const std_msgs::UInt16::ConstPtr& msg)
{
ROS_INFO("I heard: [%d]", msg->data);
error = msg->data;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "lighter");
ros::NodeHandle nh;
ros::Publisher connected =nh.advertise<std_msgs::UInt16>("/robot/sonar/head_sonar/lights/set_lights",1);
ros::Subscriber sub = nh.subscribe("/plc/error", 1000, error_sub);
std_msgs::UInt16 msg;
while(ros::ok())
{
if(error >= 0)
{
msg.data = 36863;
connected.publish(msg);
}
ros::spinOnce();
}
}
I does not work because of the bad type of the message. It should be Int8 instead of UInt16 in callback function.
Related
I created ROS Service with a client and server node. I created a ROS Service that pass the IMU sensors values from the Server to Client. And Im able to call the server and client node and get the values. But when call them with the launch file I got zero
Here the server node
#include "ros/ros.h"
#include <sensor_msgs/Imu.h>
#include "imu_service/ImuValue.h"
ros::ServiceServer service;
double current_x_orientation_s;
double get_imu_orientation_x;
bool get_val(imu_service::ImuValue::Request &req, imu_service::ImuValue::Response &res)
{
ROS_INFO("sending back response");
res.current_x_orientation_s = get_imu_orientation_x;
//.. same for the other IMU values
}
void imuCallback(const sensor_msgs::ImuConstPtr& msg)
{
current_x_orientation_s= msg->orientation.x;
get_imu_orientation_x=current_x_orientation_s;
// ..same for other IMU values
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_status_server");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/thrbot/imu", 10, imuCallback);
service = n.advertiseService("imu_status_server", get_val);
ROS_INFO("Starting server...");
ros::spin();
return 0;
}
Client
#include "ros/ros.h"
#include "ros_services/ImuValue.h"
#include <cstdlib>
ros::ServiceClient client;
int main(int argc, char **argv)
{
ros::init(argc,argv,"imu_client_node");
ros::NodeHandle n;
ros::NodeHandle nh_;
//ros::Subscriber joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("/thrbot/joy", 10, joystick_callback);
client = n.serviceClient<ros_services::ImuValue>("imu_status_server");
ros_services::ImuValue srv;
client.call(srv);
std::cout << "Got accel x: " << srv.response.current_x_orientation_s << std::endl;
return 0;
}
And the launch file
<?xml version="1.0"?>
<launch>
<node name="ImuServerService" pkg="ros_services" type="ImuServerService" output="screen">
</node>
<node name="ImuClientService" pkg="ros_services" type="ImuClientService" output="screen"/>
</launch>
I got 0 as response.
[ INFO] [1631800449.207350420]: Starting server...
[ INFO] [1631800449.212336478]: sending back response
Got accel x: 0
But when manually run the Server and the Client with
rosrun ros_services ImuServerService and rosrun ros_services ImuClientService the value is correct one
Any help?
The reason you're getting 0 back is because the client immediately calls the service on startup. Since it also immediately returns the last cached value and they're both started at almost the same time via roslaunch this means there's essentially no way a message will be received on the topic by the service is called. This works with rosrun because having to manually launch the nodes gives it enough time to actually receive something on the topic. You can observe it by adding a delay to the start of your client like so:
#include "ros/ros.h"
#include "ros_services/ImuValue.h"
#include <cstdlib>
ros::ServiceClient client;
int main(int argc, char **argv)
{
ros::init(argc,argv,"imu_client_node");
ros::NodeHandle n;
ros::NodeHandle nh_;
//ros::Subscriber joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("/thrbot/joy", 10, joystick_callback);
client = n.serviceClient<ros_services::ImuValue>("imu_status_server");
ros_services::ImuValue srv;
ros::Duration(5).sleep(); //Sleep for 5 seconds
client.call(srv);
std::cout << "Got accel x: " << srv.response.current_x_orientation_s << std::endl;
return 0;
}
Hello I am using Ubuntu 18.04 and I am very new to Ros. I was using ros kinetic. I have switched to the Ros melodic version and I am getting a compilation error in my cpp file. Below you can see my code and error I will be very happy if you could help.
this is my code:
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>
#include <string.h>
using namespace std;
/** Function Declarations **/
bool moveToGoal(double xGoal, double yGoal, double yaw);
void choose();
//code*****
void move(const std_msgs::String msg){
cout << "move Function activated" << endl;
//***code
}
int main(int argc, char** argv){
ros::init(argc, argv, "map_navigation4");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe ("server_messages", 1, move);
ros::spin();
return 0;
}
bool moveToGoal(double xGoal, double yGoal, double yaw){
//**code
}
and this error message:
enter image description here
It looks like you subscriber is not recognizing your move callback function. Below modification to your code should fix the issue-
int main(int argc, char** argv){
ros::init(argc, argv, "map_navigation4");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("server_messages", 1, moveCB);
ros::spin();
return 0;
}
And the callback function -
void moveCB(const std_msgs::String::ConstPtr &msg)
{
cout << "move Function activated" << endl;
const char *a = msg->data.c_str();
/**
* Your Code
*/
}
Reference
ROS code documentation
ROS Publisher and Subscriber Tutorial
I looked up to documantation and found a very convenient AVInputFormat's funciton get_device_list . But the problem is I can't use it.
Here's my short code snippet
#include <QDebug>
extern "C"
{
#include <libavformat/avformat.h>
#include <libavdevice/avdevice.h>
}
int main(int argc, char *argv[])
{
avdevice_register_all();
AVFormatContext *formatContext = avformat_alloc_context();
AVInputFormat *inputFormat = av_find_input_format("dshow");
AVDeviceInfoList devices;
inputFormat->get_device_list(formatContext, &devices);
qDebug() << devices.nb_devices;
avformat_free_context(formatContext);
return 0;
}
And that code CRASHES when I'm trying to print devices. How do I use that function properly? The official domentation has no examples using that function.
I've just started to work with ROS and I'm trying to write a node that publish keys in a topic.
I have created a node on a Linux Ubuntu 16.04.4 using ncurses.
This is my code:
#include <curses.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv)
{
int ch;
nodelay(stdscr, TRUE);
ros::init(argc, argv, "keyboard_driver");
ros::NodeHandle n;
ros::Publisher key_pub = n.advertise<std_msgs::String>("keys", 1);
ros::Rate loop_rate(100);
while (ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
if ((ch = getch()) != ERR)
{
ss << ch;
std::cout << ch;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
key_pub.publish(msg);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
I'm using ncurses to avoid terminal buffer.
The topic appears, but I don't get anything if, in another terminal, I run this command:
rostopic echo /keys
Debugging it I have found that getch() always return -1.
How can I do to make it work?
UPDATE
I have tried this small program, and it doesn't print anything:
#include <iostream>
#include <curses.h>
int main(int argc, char **argv)
{
int ch;
cbreak();
nodelay(stdscr, TRUE);
for(;;)
{
if ((ch = getch()) != ERR)
{
std::cout << ch;
}
}
return 0;
}
You've set nodelay so getch will return immediately with ERR if there's no data ready from the terminal. That's why getch is returning -1 (ERR). You haven't set cbreak or raw to disable terminal buffering, so you're still getting that -- no data will come from the terminal until Enter is hit.
So add a call to cbreak() at the start (just before or after the call to nodelay()) and it should work as you expect.
To use getch() you have to do the following:
#include <iostream>
#include <curses.h>
#include <signal.h>
#include <stdlib.h>
void quit(int sig)
{
endwin();
exit(0);
}
int main(int argc, char **argv)
{
int ch;
signal(SIGINT,quit);
initscr();
cbreak();
nodelay(stdscr, TRUE);
for(;;)
{
if ((ch = getch()) != ERR)
{
std::cout << ch;
}
}
return 0;
}
I forget to add initscr(); call at the beginning and endwin(); at the end of the program.
More info about how to use ncurses library here.
I'm asking you if there is a way to have a precedence between two ROS nodes. In particular, i have a ROS node that makes an output that is a text file with 60 data in it, and it recreates it every time because the data are changing. Then i have a node that has to analyze that text file. Basically, what i need is to make some changes to have a mechanism that stops the analyzer node when the writer node is running, then it has to send a signal to the analyzer node to make it able to run and analyze the text file. And then the writer node has to return let's say "in charge" to be able to re-write the text file again. So, in simple words, is a loop. Someone told me that a possible solution can be something like a "semaphore" topic in which the writer node writes, for example, a boolean value of 1 when is doing the opening, the writing and the closing of the text file, so the analyzer node knows that cannot do its elaboration, since the file is not ready yet. And, when the writer has finished and closed the text file, it has to be published a value 0 that permits the analysis by the analyzer node. I searched for the publishing of boolean values and i found a code that can be something like this:
ros::Publisher pub = n.advertise<std_msgs::Bool>("semaphore", 1000);
std_msgs::Bool state;
state.data = 1;
I don't know if i have only to use the publisher in the writer node and the subscriber in the analyzer node. Maybe i have to use both of them in the two nodes, something like: writer put a 1 in the topic semaphore so the analyzer knows that cannot access the text file, makes the text file and then put a 0 in the topic and subscribe to the topic waiting again a 1; the analyzer does something similar but in reverse. I put the two codes below, because i don't have any idea where to put the publisher and the the subscriber and how to make them working well. If possible, i have to keep this structure of working flow in my codes.
NOTE: a new text file is created almost every 10 seconds, since in the text file are written data coming from another ROS topic and the code in the writer has a mechanism to do this kind of elaboration.
Thank you in advance!!!
EDIT: Now the codes are corrected with a topic based solution as i explain in my last comment.
Writer code:
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "../include/heart_rate_monitor/wfdb.h"
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <algorithm>
#include <deque>
#include "heart_rate_monitor/analyse_heart_rate.h"
using namespace std;
static std::deque<std::string> queue_buffer;
static int entries_added_since_last_write = 0;
ros::Publisher pub;
void write_data_to_file()
{
// open file;
std::ofstream data_file("/home/marco/catkin_ws/src/heart_rate_monitor/my_data_file.txt");
if (data_file.is_open())
{
for (int i = 0; i < queue_buffer.size(); ++i)
{
data_file << queue_buffer[i] << std::endl;
}
}
else
{
std::cout << "Error - Cannot open file." << std::endl;
exit(1);
}
data_file.close();
std_msgs::Bool state;
state.data = 0;
pub.publish(state);
}
void process_message(const std_msgs::String::ConstPtr& string_msg)
{
std_msgs::Bool state;
state.data = 1;
pub.publish(state);
// if buffer has already 60 entries, throw away the oldest one
if (queue_buffer.size() == 60)
{
queue_buffer.pop_front();
}
// add the new data at the end
queue_buffer.push_back(string_msg->data);
// check if 10 elements have been added and write to file if so
entries_added_since_last_write++;
if (entries_added_since_last_write >= 10
&& queue_buffer.size() == 60)
{
// write data to file and reset counter
write_data_to_file();
entries_added_since_last_write = 0;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "writer");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/HeartRateInterval", 1000, process_message);
pub = n.advertise<std_msgs::Bool>("/semaphore", 1000);
ros::spin();
return 0;
}
Analyzer code:
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "../include/heart_rate_monitor/wfdb.h"
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <algorithm>
#include <deque>
#include "heart_rate_monitor/analyse_heart_rate.h"
void orderCallback(const std_msgs::Bool::ConstPtr& msg)
{
if (msg->data == 0)
{
chdir("/home/marco/catkin_ws/src/heart_rate_monitor");
system("get_hrv -R my_data_file.txt >doc.txt");
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "analyzer");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/semaphore", 1000, orderCallback);
ros::spin();
return 0;
}
This could be done simply using ROS services. Basically, when your node A gets the message, it does what it needs (write file) and then asks for a serice from node B (analyse the file).
The only con I see is that node A will have to wait for node B service to finish. If B dosen't need too much time, it wouldn't raise a problem.
Code Snippet :
Srv :
create a service named "analyse_heart_rate.srv" in the srv folder of your package (I supposed it's name "heart_rate_monitor").
specify the request/response of your service structure in the file:
string filename
---
bool result
CMakeLists :
add the following lines :
add_service_files(
FILES
analyse_heart_rate.srv
)
Service Server :
#include "ros/ros.h"
#include "heart_rate_monitor/analyse_heart_rate.h"
bool analyse(heart_rate_monitor::analyse_heart_rate::Request &req,
heart_rate_monitor::analyse_heart_rate::Response &res)
{
res.result = analyse_text_file(req.filename);
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "heart_rate_analyser_server");
ros::NodeHandle n;
ros::ServiceServer service = n.advertiseService("heart_rate_analyser", analyse);
ROS_INFO("Ready to analyse requests.");
ros::spin();
return 0;
}
Service Client
#include "ros/ros.h"
#include "heart_rate_monitor/analyse_heart_rate.h"
void process_message(const std_msgs::String::ConstPtr& string_msg)
{
std::string output_filename;
do_staff_with_message();
write_data_to_file_(output_filename);
heart_rate_monitor::analyse_heart_rate srv;
srv.filename = output_filename ;
if (client.call(srv))
{
ROS_INFO("Result: %d", (bool)srv.response.result);
}
else
{
ROS_ERROR("Failed to call service heart_rate_analyser");
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "add_two_ints_client");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<heart_rate_monitor::analyse_heart_rate>("heart_rate_analyser");
ros::Subscriber sub = n.subscribe("/HeartRateInterval", 1000, process_message);
return 0;
}
This way, whenever a message comes in the node "Service Client", It will process it and eventually write it to the file. Then it asks the "Service Server" to process the file created previously...
Of course, this is just a snippet, costumize it to your needs.
Cheers.