smarc-project / imc_ros_bridge Goto Github PK

Neptus expects 2 types of messages from a vehicle to ensure the plans it has are correct.
First is the PlanDBInformation. This is a simple ack of a received plan.
The second is the PlanDBState. This includes PlanDBInformations in it for every plan the vehicle knows about, with their MD5 hashes. The hashes are calculated for the PlanDBSpecification message that was previously received (it has maneuvers and such in it, we send a PlanDBInformation when we first receive it).

As it is, the MD5 hashes are easier to calculate in the bridge, since the cpp IMC library already has the serialization method we need. So right now when a PlanDB message is received, if its arg is of type PlanDBSpecification, the MD5 is calculated and passed into ROS. The rest of the system just echoes that same MD5 back to the bridge when it sends the PlanDBState message.

If the MD5s that Neptus calcualtes and the MD5 it receives in the PlanDBState message do not match, Neptus considers the plan in the vehicle out of synch and warns the user when a plan is started. The source of the warning can be found by grep -r "not synch" in Neptus source.

HOWEVER, our MD5s do not match with Neptus's. I am out of ideas and am giving up on it.
Here are some useful things to look at for any future souls that may attempt to fix this problem:

Java implementation of IMC:
https://github.com/LSTS/imcjava/blob/master/src/java/pt/lsts/imc/adapter/VehicleAdapter.java
https://github.com/LSTS/imcjava/blob/9d37d0eed8fe90f72f3a295c562c3fb53f6c7730/src/java/pt/lsts/imc/adapter/PlanDbManager.java#L65
https://github.com/LSTS/imcjava/blob/efed1384cc0cc5bacb8ce8ec7bcd536d19c91c8c/src/pt/lsts/imc/IMCMessage.java#L1592

This was received as a guide from our Porto comrades.

Good luck

We have used the imc ros bridge to convert different types of ROS messages to IMC messages, and the bridge works very well. However, we have noticed the following issues:

1. Source entity ID on the IMC messages must be hardcoded based on reserved entity IDs on the DUNE side. Is this the desired approach, or do you have another approach to set the correct entity ID?
2. For the DUNE logging task to see the IMC message from the bridge, the IMC destination address must be set to 0xFFFF, i.e., broadcast to all tasks. This is configured in udp_link.cpp (the publish_multicast() and publish() functions). These are set to 0 by default, is there a reason for this?
3. The IMC message definitions have different return types than those auto-generated using the IMC script and those that follow DUNE by default. Consequently, when adding new IMC message definitions that inherit from the base virtual functions, e.g., in Message.hpp, the return types must be manually changed. An example of this is the function: virtual double getValueFP() in Message.hpp. The proper return type of this function should be: virtual fp64_t getValueFP(). There are other examples of this, and adding new messages would be easier if the base definitions were up to date with DUNE and IMC.

Finally, we have made a 'how-to' document for integrating custom ROS messages, i.e., ROS messages that are not part of default packages such as sensor_msgs, into the imc ros bridge. In our case, we show how customized messages from an SBG INS can be added (https://github.com/SBG-Systems/sbg_ros_driver). Is this of any interest for you?

Hi,

I am trying to set up a connection between ROS and a simulated instance in Dune of the lauv.
For that, I am executing the following command in Dune:

./dune -c lauv-simulator-1 -p Simulation

Now, I want to read the IMC messages from ROS and send tasks to the robot from ROS, such as the Goto task, which is exactly what your code seems to be doing.

However, when I execute your bridge_node with the command in the tutorial

roslaunch imc_ros_bridge bridge.launch server_addr:=127.0.0.1 server_port:=6002

It seems like the connection between them never happens; the ROS code never enters in the callback functions, and the dune console never shows that a new connection has been established.
I think the problem is in the ROS parameters for the connection, e.g. the port or the robot name. I am new to Dune and Neptus, and I am unsure of how to make it work. If you could give me an example on which parameters would make the bridge work with the lauv-simulator-1, it would be much appreciated :)

Thank you!