Skip to content

Commit d07eb0c

Browse files
nilsborenilsbore
authored
Merge pull request #24 from KKalem/noetic-devel
New "Sample" maneuver and imc_id semantics changes
2 parents b60a788 + 4610a34 commit d07eb0c

File tree

83 files changed

+16793
-85
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

83 files changed

+16793
-85
lines changed

‎msg/Maneuver.msg

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
string maneuver_name
66
uint16 maneuver_imc_id
77

8+
# 450 GOTO
89
uint16 timeout
910
float64 lat
1011
float64 lon
@@ -16,3 +17,9 @@ float32 roll
1617
float32 pitch
1718
float32 yaw
1819
string custom_string
20+
21+
# 489 SAMPLE that are not in GOTO
22+
uint8 syringe0
23+
uint8 syringe1
24+
uint8 syringe2
25+

‎src/bridge_node.cpp

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -57,8 +57,10 @@ int main(int argc, char** argv)
5757
ros::param::param<std::string>("~bridge_addr", bridge_tcp_addr, "127.0.0.1");
5858
ros::param::param<std::string>("~bridge_port", bridge_tcp_port, "6002");
5959
ros::param::param<std::string>("~system_name", sys_name, "imc_ros_bridge");
60-
ros::param::param<int>("~imc_id", imc_id, 30);
61-
ros::param::param<int>("~imc_src", imc_src, 5);
60+
// Neptus seems to have swapped these values for SOME REASON. so I swap here too...
61+
// tldr: imc_id is the vehicle TYPE and imc_src is the SPECIFIC VEHICLE
62+
ros::param::param<int>("~imc_id", imc_src, 30);
63+
ros::param::param<int>("~imc_src", imc_id, 5);
6264

6365
IMCHandle imc_handle(bridge_tcp_addr, bridge_tcp_port, neptus_addr, sys_name, imc_id, imc_src);
6466

‎src/imc_handle.cpp

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -55,8 +55,10 @@ void IMCHandle::tcp_callback(const IMC::Message* msg)
5555
{
5656
uint16_t uid = msg->getId();
5757
if (callbacks.count(uid) > 0) {
58-
// 150 is a heartbeat and we dont really care about it. just debug it.
59-
if(uid == 150){
58+
// 150 is a heartbeat and we dont really care about it. just debug it.
59+
// 556 is PlanDB, i _think_ its the planDB succss, meaning "i understood that you got my plan"
60+
// neptus basically spams this so im excluding it!
61+
if(uid == 150 || uid == 556){
6062
ROS_DEBUG("Got callback with id: %u", uid);
6163
}else{
6264
ROS_INFO("Got callback with id: %u", uid);

‎src/imc_to_ros/PlanDB.cpp

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include <IMC/Spec/PlanManeuver.hpp>
2727
#include <IMC/Spec/PlanSpecification.hpp>
2828
#include <IMC/Spec/Goto.hpp>
29+
#include <IMC/Spec/Sample.hpp>
2930

3031
namespace imc_to_ros {
3132

@@ -142,7 +143,27 @@ bool convert(const IMC::PlanDB& imc_msg, imc_ros_bridge::PlanDB& ros_msg)
142143
plan_maneuver.maneuver.pitch = goto_man->pitch;
143144
plan_maneuver.maneuver.yaw = goto_man->yaw;
144145
plan_maneuver.maneuver.custom_string = goto_man->custom;
145-
} // man_id=450
146+
}
147+
// 489==Sample
148+
else if(man_id==489){
149+
IMC::Sample* sample_man = (IMC::Sample*) pm_data.get();
150+
plan_maneuver.maneuver.maneuver_name = "sample";
151+
plan_maneuver.maneuver.maneuver_imc_id = man_id;
152+
153+
plan_maneuver.maneuver.timeout = sample_man->timeout;
154+
plan_maneuver.maneuver.lat = sample_man->lat;
155+
plan_maneuver.maneuver.lon = sample_man->lon;
156+
plan_maneuver.maneuver.z = sample_man->z;
157+
plan_maneuver.maneuver.z_units = sample_man->z_units;
158+
plan_maneuver.maneuver.speed = sample_man->speed;
159+
plan_maneuver.maneuver.speed_units = sample_man->speed_units;
160+
// hardcoded 3 syringes... in the spec! BAH!
161+
plan_maneuver.maneuver.syringe0 = sample_man->syringe0;
162+
plan_maneuver.maneuver.syringe1 = sample_man->syringe1;
163+
plan_maneuver.maneuver.syringe2 = sample_man->syringe2;
164+
plan_maneuver.maneuver.custom_string = sample_man->custom;
165+
166+
}
146167
else{
147168
std::cout << "Maneuver not implemented! id:" << man_id << std::endl;
148169
}

‎vehicles-defs/00-lolo-auv.nvcl

Lines changed: 98 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,10 @@
88
<x-size>3</x-size>
99
<y-size>1</y-size>
1010
<z-size>1</z-size>
11-
<top-image-2D>../vehicles-files/sam/sam_top.png</top-image-2D>
12-
<side-image-2D>../vehicles-files/sam/sam_side.png</side-image-2D>
13-
<presentation-image-2D>../vehicles-files/lolo/lolo_pres.png</presentation-image-2D>
14-
<model-3D>../vehicles-files/adamastor/adamastor.j3d</model-3D>
11+
<top-image-2D>lolo_top.png</top-image-2D>
12+
<side-image-2D>lolo_side.png</side-image-2D>
13+
<presentation-image-2D>lolo.png</presentation-image-2D>
14+
<model-3D>lolo.obj</model-3D>
1515
<icon-color>
1616
<r>235</r>
1717
<g>192</g>
@@ -53,88 +53,111 @@
5353
</annotation>
5454
</CoverArea>
5555
</maneuver>
56-
<maneuver>
57-
<Loiter kind="automatic">
58-
<basePoint type="pointType">
59-
<point>
60-
<id>LoiterLoc</id>
61-
<name>LoiterLoc</name>
62-
<coordinate>
63-
<latitude>0N</latitude>
64-
<longitude>0E</longitude>
65-
<depth>60</depth>
66-
</coordinate>
67-
</point>
68-
<radiusTolerance>5</radiusTolerance>
69-
</basePoint>
70-
<duration>30</duration>
71-
<trajectory>
72-
<radius type="float">10</radius>
73-
<radiusTolerance type="float">5</radiusTolerance>
74-
<type>Circular</type>
75-
<length type="float">50</length>
76-
<bearing type="float">0</bearing>
77-
<direction>Clockwise</direction>
78-
</trajectory>
79-
<velocity type="float" unit="RPM" tolerance="5">900</velocity>
80-
<annotation>
81-
<documentation>Loiter over a point</documentation>
82-
<implementation-class>pt.lsts.neptus.mp.maneuvers.Loiter</implementation-class>
83-
</annotation>
84-
</Loiter>
85-
</maneuver>
86-
<maneuver>
87-
<StationKeeping kind="automatic">
88-
<basePoint type="pointType">
89-
<point>
90-
<id>SKLoc</id>
91-
<name>SKLoc</name>
92-
<coordinate>
93-
<latitude>0N</latitude>
94-
<longitude>0E</longitude>
95-
<depth>60</depth>
96-
</coordinate>
97-
</point>
98-
<radiusTolerance>5</radiusTolerance>
99-
</basePoint>
100-
<duration>0</duration>
101-
<trajectory>
102-
<radius type="float">10</radius>
103-
</trajectory>
104-
<speed type="float" unit="RPM" tolerance="5">900</speed>
105-
<annotation>
106-
<documentation>Station Keeping</documentation>
107-
<implementation-class>pt.lsts.neptus.mp.maneuvers.StationKeeping</implementation-class>
108-
</annotation>
109-
</StationKeeping>
110-
</maneuver>
111-
<maneuver>
112-
<Elevator>
56+
<maneuver>
57+
<Sample kind="automatic">
11358
<finalPoint type="pointType">
11459
<point>
115-
<id>Elevator-Point</id>
60+
<id>SampleDestination</id>
61+
<name>SampleDestination</name>
11662
<coordinate>
11763
<latitude>0N</latitude>
11864
<longitude>0E</longitude>
119-
<depth>0</depth>
65+
<height>2</height>
12066
</coordinate>
12167
</point>
122-
<radiusTolerance>0</radiusTolerance>
68+
<radiusTolerance>1</radiusTolerance>
12369
</finalPoint>
124-
<startZ>2</startZ>
125-
<startZUnits>DEPTH</startZUnits>
126-
<pitch>30</pitch>
127-
<radius>10</radius>
128-
<duration>0</duration>
129-
<speed unit="RPM">1000.0</speed>
70+
<velocity tolerance="10" type="float" unit="m/s">5</velocity>
71+
<trajectoryTolerance>
72+
<radiusTolerance type="float">1</radiusTolerance>
73+
</trajectoryTolerance>
13074
<annotation>
131-
<documentation>No documentation available</documentation>
132-
<implementation-class>pt.lsts.neptus.mp.maneuvers.Elevator</implementation-class>
75+
<documentation>Move to a given point and start taking samples until the next sample WP.</documentation>
76+
<implementation-class>pt.lsts.neptus.mp.maneuvers.Sample</implementation-class>
13377
</annotation>
134-
</Elevator>
78+
</Sample>
13579
</maneuver>
136-
137-
80+
<!--These are not implemented in the bridge-->
81+
<!--<maneuver>-->
82+
<!--<Loiter kind="automatic">-->
83+
<!--<basePoint type="pointType">-->
84+
<!--<point>-->
85+
<!--<id>LoiterLoc</id>-->
86+
<!--<name>LoiterLoc</name>-->
87+
<!--<coordinate>-->
88+
<!--<latitude>0N</latitude>-->
89+
<!--<longitude>0E</longitude>-->
90+
<!--<depth>60</depth>-->
91+
<!--</coordinate>-->
92+
<!--</point>-->
93+
<!--<radiusTolerance>5</radiusTolerance>-->
94+
<!--</basePoint>-->
95+
<!--<duration>30</duration>-->
96+
<!--<trajectory>-->
97+
<!--<radius type="float">10</radius>-->
98+
<!--<radiusTolerance type="float">5</radiusTolerance>-->
99+
<!--<type>Circular</type>-->
100+
<!--<length type="float">50</length>-->
101+
<!--<bearing type="float">0</bearing>-->
102+
<!--<direction>Clockwise</direction>-->
103+
<!--</trajectory>-->
104+
<!--<velocity type="float" unit="RPM" tolerance="5">900</velocity>-->
105+
<!--<annotation>-->
106+
<!--<documentation>Loiter over a point</documentation>-->
107+
<!--<implementation-class>pt.lsts.neptus.mp.maneuvers.Loiter</implementation-class>-->
108+
<!--</annotation>-->
109+
<!--</Loiter>-->
110+
<!--</maneuver>-->
111+
<!--<maneuver>-->
112+
<!--<StationKeeping kind="automatic">-->
113+
<!--<basePoint type="pointType">-->
114+
<!--<point>-->
115+
<!--<id>SKLoc</id>-->
116+
<!--<name>SKLoc</name>-->
117+
<!--<coordinate>-->
118+
<!--<latitude>0N</latitude>-->
119+
<!--<longitude>0E</longitude>-->
120+
<!--<depth>60</depth>-->
121+
<!--</coordinate>-->
122+
<!--</point>-->
123+
<!--<radiusTolerance>5</radiusTolerance>-->
124+
<!--</basePoint>-->
125+
<!--<duration>0</duration>-->
126+
<!--<trajectory>-->
127+
<!--<radius type="float">10</radius>-->
128+
<!--</trajectory>-->
129+
<!--<speed type="float" unit="RPM" tolerance="5">900</speed>-->
130+
<!--<annotation>-->
131+
<!--<documentation>Station Keeping</documentation>-->
132+
<!--<implementation-class>pt.lsts.neptus.mp.maneuvers.StationKeeping</implementation-class>-->
133+
<!--</annotation>-->
134+
<!--</StationKeeping>-->
135+
<!--</maneuver>-->
136+
<!--<maneuver>-->
137+
<!--<Elevator>-->
138+
<!--<finalPoint type="pointType">-->
139+
<!--<point>-->
140+
<!--<id>Elevator-Point</id>-->
141+
<!--<coordinate>-->
142+
<!--<latitude>0N</latitude>-->
143+
<!--<longitude>0E</longitude>-->
144+
<!--<depth>0</depth>-->
145+
<!--</coordinate>-->
146+
<!--</point>-->
147+
<!--<radiusTolerance>0</radiusTolerance>-->
148+
<!--</finalPoint>-->
149+
<!--<startZ>2</startZ>-->
150+
<!--<startZUnits>DEPTH</startZUnits>-->
151+
<!--<pitch>30</pitch>-->
152+
<!--<radius>10</radius>-->
153+
<!--<duration>0</duration>-->
154+
<!--<speed unit="RPM">1000.0</speed>-->
155+
<!--<annotation>-->
156+
<!--<documentation>No documentation available</documentation>-->
157+
<!--<implementation-class>pt.lsts.neptus.mp.maneuvers.Elevator</implementation-class>-->
158+
<!--</annotation>-->
159+
<!--</Elevator>-->
160+
<!--</maneuver>-->
138161
</feasibleManeuvers>
139162
<communication-means>
140163
<comm-mean>

‎vehicles-defs/00-sam-auv.nvcl

Lines changed: 29 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,10 @@
88
<x-size>1.2</x-size>
99
<y-size>0.75</y-size>
1010
<z-size>0.7</z-size>
11-
<top-image-2D>../vehicles-files/sam/sam_top.png</top-image-2D>
12-
<side-image-2D>../vehicles-files/sam/sam_side.png</side-image-2D>
13-
<presentation-image-2D>../vehicles-files/sam/sam_pres.png</presentation-image-2D>
14-
<model-3D>../vehicles-files/adamastor/adamastor.j3d</model-3D>
11+
<top-image-2D>sam_top.png</top-image-2D>
12+
<side-image-2D>sam_side.png</side-image-2D>
13+
<presentation-image-2D>sam.png</presentation-image-2D>
14+
<model-3D>sam.obj</model-3D>
1515
<icon-color>
1616
<r>222</r>
1717
<g>201</g>
@@ -40,10 +40,34 @@
4040
<radiusTolerance type="float">1</radiusTolerance>
4141
</trajectoryTolerance>
4242
<annotation>
43-
<documentation>No documentation available</documentation>
43+
<documentation>Move to a point.</documentation>
4444
<implementation-class>pt.lsts.neptus.mp.maneuvers.Goto</implementation-class>
4545
</annotation>
4646
</Goto>
47+
</maneuver>
48+
<maneuver>
49+
<Sample kind="automatic">
50+
<finalPoint type="pointType">
51+
<point>
52+
<id>SampleDestination</id>
53+
<name>SampleDestination</name>
54+
<coordinate>
55+
<latitude>0N</latitude>
56+
<longitude>0E</longitude>
57+
<height>2</height>
58+
</coordinate>
59+
</point>
60+
<radiusTolerance>1</radiusTolerance>
61+
</finalPoint>
62+
<velocity tolerance="10" type="float" unit="m/s">5</velocity>
63+
<trajectoryTolerance>
64+
<radiusTolerance type="float">1</radiusTolerance>
65+
</trajectoryTolerance>
66+
<annotation>
67+
<documentation>Move to a given point and start taking samples until the next sample WP.</documentation>
68+
<implementation-class>pt.lsts.neptus.mp.maneuvers.Sample</implementation-class>
69+
</annotation>
70+
</Sample>
4771
</maneuver>
4872
</feasibleManeuvers>
4973
<communication-means>

‎vehicles-defs/defaults/04-adamastor-vehicle-lsts.nvcl

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<system xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="..\src\schemas\neptus-vehicle.xsd">
3+
<properties>
4+
<id>adamastor</id>
5+
<name>Adamastor</name>
6+
<type>ROV</type>
7+
<appearance>
8+
<x-size>1.2</x-size>
9+
<y-size>0.75</y-size>
10+
<z-size>0.7</z-size>
11+
<top-image-2D>../vehicles-files/adamastor/adamastor_top.png</top-image-2D>
12+
<side-image-2D>../vehicles-files/adamastor/adamastor_side.png</side-image-2D>
13+
<presentation-image-2D>../vehicles-files/adamastor/adamastor_presentation.png</presentation-image-2D>
14+
<model-3D>../vehicles-files/adamastor/adamastor.j3d</model-3D>
15+
</appearance>
16+
<coordinate-system-label>SNAME</coordinate-system-label>
17+
</properties>
18+
<feasibleManeuvers>
19+
<maneuver>
20+
<Unconstrained kind="manual">
21+
<annotation>
22+
<documentation>No documentation available</documentation>
23+
<implementation-class>pt.lsts.neptus.mp.maneuvers.Unconstrained</implementation-class>
24+
</annotation>
25+
</Unconstrained>
26+
</maneuver>
27+
</feasibleManeuvers>
28+
<communication-means>
29+
<comm-mean>
30+
<name>ethernet</name>
31+
<type>ethernet</type>
32+
<host-address>10.0.10.70</host-address>
33+
<protocols>imc</protocols>
34+
<latency value="100M" unit="bps"/>
35+
<protocols-args>
36+
<imc>
37+
<port>6002</port>
38+
<port-tcp>6002</port-tcp>
39+
<udp-on>true</udp-on>
40+
<tcp-on>false</tcp-on>
41+
<imc-id>04:02</imc-id>
42+
</imc>
43+
</protocols-args>
44+
</comm-mean>
45+
</communication-means>
46+
<configuration-files>
47+
</configuration-files>
48+
<consoles>
49+
<console name="Adamastor Console">
50+
<xml-file>../conf/consoles/adamastor-light.ncon</xml-file>
51+
</console>
52+
</consoles>
53+
</system>

0 commit comments

Comments
 (0)