ROS tf.transform cannot find a frame which actually exists (can be traced with rosrun tf tf_echo) - ubuntu-16.04

Doe someone experienced with ROS the following behavior: how is it possible that a tf.lookupTransform("map", "base_link", ros::Time(0), transform);
tells you that: "base_link" passed to lookupTransform argument target_frame does not exist but if i enter:
rosrun tf tf_echo base_link map
At time 1549633095.937
- Translation: [-0.005, 0.020, -0.129]
- Rotation: in Quaternion [0.033, 0.063, 0.002, 0.997]
in RPY (radian) [0.066, 0.127, 0.009]
I can see that the frame not only exist but that there is an effective transformation available between map and base_link?
I am totally clueless about such weird behavior. Any help will be very very welcome. Indeed the program is working on my laptop but not on an Intel NUC.
The complete piece of code is given below (actually I have a segmentation fault during the creation of the costmap_2d::costmap2DROS and looks like it is because tf.transform is failing):
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
int main(int argc, char **argv) {
ros::init(argc, argv, "hector_exploration_node");
ros::NodeHandle nh;
tf::TransformListener tf;
tf::StampedTransform transform;
try{
tf.lookupTransform("/base_link", "/map", ros::Time(0), transform);
std::cout << "transform exist\n";
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
std::cout << "before costmap\n";
costmap_2d::Costmap2DROS costmap("global_costmap", tf);
costmap.start();
ros::Rate rate(10);
while (ros::ok())
ros::spin();
return 0;
}

You try to perform the transformation immediately after you've created your tf listener, which is commonly a bad practice for the following reason: The listener's buffer, which carries all information about recent transformation, is literally empty. Therefore, any transform which looks-up the buffer does not find the frames it needs. It is good practice to wait for some time after the listener has been created so that the buffer can fill up. But instead of just sleeping, tf comes with its own implementation to wait for exactly the frames you are asking for: waitForTransform. It can be used as explained here. Therefore, you just have to extend your try block as follows:
try{
tf.waitForTransform("/base_link", "/map", ros::Time(0), ros::Duration(3.0));
tf.lookupTransform("/base_link", "/map", ros::Time(0), transform);
std::cout << "transform exist\n";
}

Related

pcap_getnonblock() returns -3

I am quite new to using pcap lib, so please bear with me.
I am trying to use pcap_getnonblock function, the documentation says the following:
pcap_getnonblock() returns the current 'non-blocking' state of
the capture descriptor; it always returns 0 on 'savefiles' . If
there is an error, PCAP_ERROR is returned and errbuf is filled in
with an appropriate error message.
errbuf is assumed to be able to hold at least PCAP_ERRBUF_SIZE
chars.
I got -3 returned and the errbuf is an empty string, I couldn't understand the meaning of such result.
I believe this caused a socket error: 10065.
This problem happened only once and I could not reproduce it, but still it would be great to find its causing to prevent it in future executions.
Thanks in advance.
pcap_getnonblock() can return -3 - that's PCAP_ERROR_NOT_ACTIVATED. Unfortunately, that's not documented; I'll fix that.
Here's a minimal reproducible example that demonstrates this:
#include <pcap/pcap.h>
#include <stdio.h>
int
main(int argc, char **argv)
{
pcap_t *pcap;
char errbuf[PCAP_ERRBUF_SIZE];
if (argc != 2) {
fprintf(stderr, "Usage: this_program <interface_name>\n");
return 1;
}
pcap = pcap_create(argv[1], errbuf);
if (pcap == NULL) {
fprintf(stderr, "this_program: pcap_create(%s) failed: %s\n",
argv[1], errbuf);
return 2;
}
printf("pcap_getnonblock() returns %d on non-activated pcap_t\n",
pcap_getnonblock(pcap, errbuf));
return 0;
}
(yes, that's minimal, as 1) names of interfaces are OS-dependent, so it has to be a command-line argument and 2) if you don't run the program correctly, it should let you know what's happening, so you know what you have to do in order to reproduce the problem).
Perhaps pcap_getnonblock() and pcap_setnonblock() should be changed so that you can set non-blocking mode before activating the pcap_t, so that, when activated, it will be in non-blocking mode. It doesn't work that way currently, however.
I.e., you're allocating a pcap_t with pcap_create(), but you're not activating it with pcap_activate(). You need to do both in order to have a pcap_t on which you can capture.

How to import .ply files using PCL io::loadPLYFile?

When importing a ply-file into my program I get an Error-message saying that something went wrong with the following message:
C:\Users\...\data\apple.ply:8: property 'list uint8 int32 vertex_indices' of element 'face' is not handled
I used a sample ply file from: https://people.sc.fsu.edu/~jburkardt/data/ply/apple.ply
I have already tried different ply files from different sources but none of them work. When debugging the program the io::loadPLYFile doesn't generate a valid pointcloud. Runtime Library for PCL and for my program are the same.
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/surface/marching_cubes_rbf.h>
using namespace pcl;
using namespace std;
int
main (int argc, char** argv)
{
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
std::cout << "Start Debug?" << std::endl;
std::cin.ignore();
if(io::loadPLYFile<PointXYZ> (argv[1], *cloud) == -1){
cout << "ERROR: couldn't find file" << endl;
return (1);
} else {
cout << "loaded" << endl;
NormalEstimationOMP<PointXYZ, Normal> ne;
search::KdTree<PointXYZ>::Ptr tree1 (new search::KdTree<PointXYZ>);
tree1->setInputCloud (cloud);
ne.setInputCloud (cloud);
ne.setSearchMethod (tree1);
ne.setKSearch (20);
PointCloud<Normal>::Ptr normals (new PointCloud<Normal>);
ne.compute (*normals);
I would expect the PCL function io::loadPLYFile to load the files properly as described in the documentation http://docs.pointclouds.org/1.3.1/group__io.html
the console output is just a warning as #kanstar already suggested! It can easily be ignored. The reason my program crashed in Debug but not in Release was that my Visual Studio linked to the wrong library version of boost which resulted in the crash. Fixing the linkage made the pcl::NormalEstimationOMP work as expected.

a lack of examples of using libmodbus functions

I am new to modbus. I have spent hours reading the Help(?) files, which never seem to give you an example! I am using C on a Raspberry Pi, model3 and have installed libmodbus. I am trying to talk to an epSolar solar panel controller via an FTDI USB to RS485 converter.
The epSolar docs say that the Read Input registers start at address 3000 and continue to 311D. I am trying to read 3104.
I modified the code below. It connects to the device but trying to read input register 0x04 always returns -1:
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <modbus.h>
enum {TCP, RTU};
int main(int argc, char *argv[])
{
int socket;
modbus_t *ctx;
modbus_mapping_t *mb_mapping;
int rc;
int use_backend;
int i;
uint16_t tab_reg[64];
use_backend = RTU;
printf("Waiting for Serial connection\n");
ctx = modbus_new_rtu("/dev/SOLAR", 115200, 'N', 8, 1);
modbus_set_slave(ctx, 0);
//modbus_connect(ctx);
if(modbus_connect(ctx) == -1)
{
fprintf(stderr, "Serial connection failed:
%s\n", modbus_strerror(errno));
modbus_free(ctx);
return -1;
}
printf("Serial connection started!\n");
mb_mapping = modbus_mapping_new(MODBUS_MAX_READ_BITS, 0,
MODBUS_MAX_READ_REGISTERS, 0);
if(mb_mapping == NULL)
{
fprintf(stderr, "Failed to allocate the mapping: %s\n",
modbus_strerror(errno));
modbus_free(ctx);
return -1;
}
rc = modbus_read_input_registers(ctx, 1, 0x0A, tab_reg);
if(rc == -1)
{
fprintf(stderr, "%s\n", modbus_strerror(errno));
return -1;
}
for(i=0; i < rc; i++)
printf("reg[%d]=%d (0x%X)\n", i, tab_reg[i], tab_reg[i]);
modbus_mapping_free(mb_mapping);
modbus_free(ctx);
modbus_close(ctx);
return 0;
}
It connects fine and allocates the mapping, but rc is always -1 with error message that the port has timed out.
I have run out of ideas and feel like I am navigating through treacle!
Any help most appreciated.
I am also new to Modbus. With my current experience, make sure you are allocating enough memory for the tab_reg for storing the results. Also try setting the Debug mode on i.e modbus_set_debug(ctx, TRUE); to Check for the request and response code.
I know this is a really old question, but hopefully this answer will help anyone who lands here via a Google search.
I can see a few points that need some help.
As commented by Saad above, the modbus server ID above is incorrect. ID 0 is reserved for broadcast messages, which a slave will not respond to. Find out what the Modbus ID for the target device is, and use that.
I think what's tricking you is that you'll also always get a proper "connect" as long as the serial port you provided is valid. This isn't a connection to any particular device so much as it's a connection to the Modbus network port. You're getting a timeout because a response was expected by libmodbus, but no response was received on the wire.
There are several other little troubles in the code presented, but given the age of this post I almost feel like I'm nitpicking something the OP probably already solved. The big problem is the unworkable slave ID. Other minor problems include: unnecessary use of modbus_mapping (struct for use on server/slaves), possible misallocation of modbus_mapping (no space allocated for input registers).

libpcap not receiving in real time, seems to be buffering packets

So I'm working with a device where I need to send and receive raw ethernet frames. It's a wireless radio and it uses ethernet to send status messages to its host. The protocol it uses is actually IPX, but I figured it would be easier to send raw ethernet frames using libpcap than to dig through decades old code implementing IPX (which got replaced by TCP/IP, so it's quite old).
My program sends a request packet (this packet is exactly the same every time, it's stateless) and the device returns a response packet with the data I need. I'm using pcap_inject to send the frame and pcap_loop in another thread to do the receiving. I originally had it in one thread, but tried 2 threads to see if it fixed the issue I'm having.
The issue is that libpcap doesn't seem to be receiving the packets in real time. It seems to buffer about 5 of them and then process them all at once. I want to be able to read them as fast as they come. Is there some way to disable this buffering on libpcap, or increase the refresh rate?
Some example output (I just printed out the time that a packet was received). Notice how there is about a second of time between each group
Time: 1365792602.805750
Time: 1365792602.805791
Time: 1365792602.805806
Time: 1365792602.805816
Time: 1365792602.805825
Time: 1365792602.805834
Time: 1365792603.806886
Time: 1365792603.806925
Time: 1365792603.806936
Time: 1365792603.806944
Time: 1365792603.806952
Time: 1365792604.808007
Time: 1365792604.808044
Time: 1365792604.808055
Time: 1365792604.808063
Time: 1365792604.808071
Time: 1365792605.809158
Time: 1365792605.809194
Time: 1365792605.809204
Time: 1365792605.809214
Time: 1365792605.809223
Here's the inject code:
char errbuf[PCAP_ERRBUF_SIZE];
char *dev="en0";
if(dev==NULL){
fprintf(stderr,"Pcap error: %s\n",errbuf);
return 2;
}
printf("Device: %s\n",dev);
pcap_t *handle;
handle=pcap_open_live(dev, BUFSIZ, 1, 1000, errbuf);
if(handle==NULL){
fprintf(stderr, "Device open error: %s\n",errbuf);
return 2;
}
//Construct the packet that will get sent to the radio
struct ether_header header;
header.ether_type=htons(0x0170);
int i;
for(i=0;i<6;i++){
header.ether_dhost[i]=radio_ether_address[i];
header.ether_shost[i]=my_ether_address[i];
}
unsigned char frame[sizeof(struct ether_header)+sizeof(radio_request_packet)];
memcpy(frame, &header, sizeof(struct ether_header));
memcpy(frame+sizeof(struct ether_header), radio_request_packet, sizeof(radio_request_packet));
if(pcap_inject(handle, frame, sizeof(frame))==-1){
pcap_perror(handle, errbuf);
fprintf(stderr, "Couldn't send frame: %s\n",errbuf);
return 2;
}
bpf_u_int32 mask;
bpf_u_int32 net;
if(pcap_lookupnet(dev,&net,&mask,errbuf)==-1){
pcap_perror(handle, errbuf);
fprintf(stderr,"Net mask error: %s\n",errbuf);
return 2;
}
char *filter="ether src 00:30:30:01:b1:35";
struct bpf_program fp;
if(pcap_compile(handle, &fp, filter, 0, net)==-1){
pcap_perror(handle, errbuf);
fprintf(stderr,"Filter error: %s\n",errbuf);
return 2;
}
if(pcap_setfilter(handle, &fp)==-1){
pcap_perror(handle, errbuf);
fprintf(stderr, "Install filter error: %s\n",errbuf);
return 2;
}
printf("Starting capture\n");
pthread_t recvThread;
pthread_create(&recvThread, NULL, (void *(*)(void *))thread_helper, handle);
while(1){
if(pcap_inject(handle, frame, sizeof(frame))==-1){
pcap_perror(handle, errbuf);
fprintf(stderr, "Couldn't inject frame: %s\n",errbuf);
return 2;
}
usleep(200000);
}
pcap_close(handle);
return 0;
And the receiving code:
void got_packet(u_char *args,const struct pcap_pkthdr * header,const u_char * packet){
struct timeval tv;
gettimeofday(&tv, NULL);
double seconds=(double)tv.tv_sec + ((double)tv.tv_usec)/1000000.0;
printf("Time: %.6f\n",seconds);
}
void *thread_helper(pcap_t *handle){
pcap_loop(handle, -1, got_packet, NULL);
return NULL;
}
Is there some way to disable this buffering on libpcap
There's currently no libpcap API to do that.
However, depending on what OS you're running, there may be ways to do it for that particular OS, i.e. you can do it, but in a non-portable fashion.
For systems that use BPF, including *BSD and...
...OS X, which, given the "en0", I suspect you're using, the way to do it is to do something such as:
Creating a set_immediate_mode.h header file containing:
extern int set_immediate_mode(int fd);
Creating a set_immediate_mode.c source file containing:
#include <sys/types.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <net/bpf.h>
#include "set_immediate_mode.h"
int
set_immediate_mode(int fd)
{
int on = 1;
return ioctl(fd, BIOCIMMEDIATE, &on);
}
Adding #include <string.h> and #include <errno.h> to your program if it's not already including those files, adding #include "set_immediate_mode.h" to your program, and adding, after the pcap_open_live() call succeeds, the following code:
int fd;
fd = pcap_fileno(handle);
if (fd == -1) {
fprintf(stderr, "Can't get file descriptor for pcap_t (this should not happen)\n");
return 2;
}
if (set_immediate_mode(fd) == -1) {
fprintf(stderr, "BIOCIMMEDIATE failed: %s\n", strerror(errno));
return 2;
}
That will completely disable the buffering that BPF normally does (that's the buffering you're seeing with libpcap; see the BPF(4) man page), so that packets are delivered as soon as they arrive. That changes the way buffering is done in ways that might cause BPF's internal buffers to fill up faster than they would if the normal buffering is done, so that might cause packets to be lost when they wouldn't otherwise be lost, but using pcap_set_buffer_size(), as suggested by Kiran Bandla, could help that if it happens (which it might not, especially given that you're using a filter to keep "uninteresting" packets from being put into BPF's buffer in the first place).
On Linux, this is currently not necessary - what buffering is done doesn't have a timeout for the delivery of packets. On Solaris, it would be done similarly on Solaris 11 (as libpcap uses BPF), but would be done differently on earlier versions of Solaris (as they didn't have BPF and libpcap uses DLPI). On Windows with WinPcap, pcap_open() has a flag for that.
A future version of libpcap will probably have an API for this; I can't promise when that will happen.
You can set the capture buffer size by using pcap_set_buffer_size. Make sure you do this before you activate your capture handle.
Lowering the buffer size is not always a good idea. Watchout for your CPU and also dropped packets at high capture rate.

ncurses and stdin blocking

I have stdin in a select() set and I want to take a string from stdin whenever the user types it and hits Enter.
But select is triggering stdin as ready to read before Enter is hit, and, in rare cases, before anything is typed at all. This hangs my program on getstr() until I hit Enter.
I tried setting nocbreak() and it's perfect really except that nothing gets echoed to the screen so I can't see what I'm typing. And setting echo() doesn't change that.
I also tried using timeout(0), but the results of that was even crazier and didn't work.
What you need to do is tho check if a character is available with the getch() function. If you use it in no-delay mode the method will not block. Then you need to eat up the characters until you encounter a '\n', appending each char to the resulting string as you go.
Alternatively - and the method I use - is to use the GNU readline library. It has support for non-blocking behavior, but documentation about that section is not so excellent.
Included here is a small example that you can use. It has a select loop, and uses the GNU readline library:
#include <stdio.h>
#include <readline/readline.h>
#include <readline/history.h>
#include <stdlib.h>
#include <stdbool.h>
int quit = false;
void rl_cb(char* line)
{
if (NULL==line) {
quit = true;
return;
}
if(strlen(line) > 0) add_history(line);
printf("You typed:\n%s\n", line);
free(line);
}
int main()
{
struct timeval to;
const char *prompt = "# ";
rl_callback_handler_install(prompt, (rl_vcpfunc_t*) &rl_cb);
to.tv_sec = 0;
to.tv_usec = 10000;
while(1){
if (quit) break;
select(1, NULL, NULL, NULL, &to);
rl_callback_read_char();
};
rl_callback_handler_remove();
return 0;
}
Compile with:
gcc -Wall rl.c -lreadline