diff --git a/backpack_pkg/CMakeLists.txt b/backpack_pkg/CMakeLists.txt
index 7201da40e3015919beddb9cf11bbe8cbc135aa6f..06350a7d6a5c191a2de2634daf69a85f8967a206 100644
--- a/backpack_pkg/CMakeLists.txt
+++ b/backpack_pkg/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS
velodyne_msgs
velodyne_pointcloud
velodyne_gps_imu
+ velodyne_gps_imu
xsens_mti_driver
robot_localization
)
@@ -31,7 +32,7 @@ find_package(Boost REQUIRED COMPONENTS system)
###################################
catkin_package(
- INCLUDE_DIRS /home/mona/deep_ws
+ INCLUDE_DIRS /home/deepforest/catkin_ws
LIBRARIES backpack_pkg
CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs nav_msgs geometry_msgs velodyne_driver velodyne_laserscan velodyne_pcl velodyne_msgs velodyne_pointcloud velodyne_gps_imu xsens_mti_driver robot_localization
DEPENDS
@@ -43,7 +44,7 @@ catkin_package(
## Specify additional locations of header files
## Your package locations should be listed before other locations
-include_directories(~/deep_ws/src/backpack_pkg ~/deep_ws/src/velodyne ~/deep_ws/src/xsens_mti_driver /opt/ros/noetic/lib)
+include_directories(~/catkin_ws/src/backpack_pkg ~/catkin_ws/src/velodyne ~/catkin_ws/src/xsens_mti_driver /opt/ros/noetic/lib)
# include ${catkin_INCLUDE_DIRS}
diff --git a/backpack_pkg/launch/assembler.py b/backpack_pkg/launch/assembler.py
new file mode 100755
index 0000000000000000000000000000000000000000..434fd491ff10ea5dd44659cf793fce786c0c1eb1
--- /dev/null
+++ b/backpack_pkg/launch/assembler.py
@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+
+
+# adapted from tutorial by "The Construct" and example on laser_assembler documentation
+
+import roslib; roslib.load_manifest('laser_assembler')
+import rospy; from laser_assembler.srv import *
+from sensor_msgs.msg import PointCloud2
+
+rospy.init_node("assemble_client")
+rospy.wait_for_service("/assemble_scans2")
+assemble_cloud = rospy.ServiceProxy('/assemble_scans2', AssembleScans2)
+pub = rospy.Publisher("/vlp2_cloud", PointCloud2, queue_size=1)
+r= rospy.Rate(1)
+
+while not rospy.is_shutdown():
+ try:
+ resp = assemble_cloud(rospy.Time(0,0), rospy.get_rostime())
+ print("Got cloud with %u points" % len(resp.cloud.data))
+ pub.publish(resp.cloud)
+
+ except rospy.ServiceException(e):
+ print("Service call failed: %s" %e)
+
+ r.sleep()
diff --git a/backpack_pkg/launch/backpack_navigation.launch b/backpack_pkg/launch/backpack_navigation.launch
index da99ba832c19f688c9777e0d20f42d87b2c08cf6..c2bce66b73cd7964b421abec05154cf1ddb9e7c2 100755
--- a/backpack_pkg/launch/backpack_navigation.launch
+++ b/backpack_pkg/launch/backpack_navigation.launch
@@ -3,7 +3,7 @@
<launch>
<arg name="record" default="false" />
- <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp201" args="-0.02 0 0.83 0 0.785 0 /base_link /vlp201 100" />
+ <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp201" args="-0.02 0 0.83 3.14 0.785 0 /base_link /vlp201 100" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp202" args="-0.37 0 -0.15 0 -1.57 0 /base_link /vlp202 100" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_imu" args="0.0 0 0.0 0 0 0 /base_link /imu_link 100" />
@@ -18,26 +18,37 @@
<arg name="ns_lidar" value="vlp1" />
</include>
- <!-- run ekf nodes: (1) EKF: odom > base_link (2) EKF: map > odom (depends on (1); Navsat GPS > map -->
+ <!-- run ekf nodes: (1) EKF: odom > base_link (2) EKF: map > odom (depends on (1)); Navsat GPS > map -->
<rosparam command="load" file="$(find backpack_pkg)/params/ekf_navsat.yaml" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
+ <remap from="odometry/filtered" to="odometry/filtered"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
- <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
+ <!-- Node for the GPS attached on IMU -->
+ <node pkg="robot_localization" type="navsat_transform_node" name="navsat_imu" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
- <remap from="gps/fix" to="gnss"/>
- <remap from="imu/data" to="imu/data"/>
- </node>
+ <remap from="/imu/data" to="/imu/data"/>
+ <remap from="/gps/fix" to="/gnss"/>
+ </node>
+
+ <!-- Node for GPS attached on Laser
+ <node pkg="robot_localization" type="navsat_transform_node" name="navsat_laser" clear_params="true" ns="vlp1">
+ <remap from="/odometry/filtered" to="/odometry/filtered_map"/>
+ <remap from="/imu/data" to="/vlp1/gpsimu_driver/imu_data"/>
+ <remap from="/gps/fix" to="/vlp1/gpsimu_driver/nmea_sentence"/>
+ </node> -->
- <!-- draw path from odometry
- <node pkg="backpack_pkg" type="odom_to_path.py" name="draw_path" /> -->
+
+ <include file="$(find backpack_pkg)/launch/laser_assemble.launch" />
+ <include file="$(find backpack_pkg)/launch/gps_velo_pub.launch" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find backpack_pkg)/launch/display.rviz" required="true" />
+
</launch>
diff --git a/backpack_pkg/launch/backpack_sensors.launch b/backpack_pkg/launch/backpack_sensors.launch
index 4de593cfe9081b2543df422f8490585ea8d52c4e..b392dc0e04b8b14327ba6b5bbac74612ab95325a 100755
--- a/backpack_pkg/launch/backpack_sensors.launch
+++ b/backpack_pkg/launch/backpack_sensors.launch
@@ -6,7 +6,8 @@
<include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg name="device_ip" value="192.168.1.201"/> <!--args used when two Velodynes are implemented -->
<arg name="frame_id" value="vlp201"/>
- <arg name="port" value="2368"/>
+ <arg name="port" value="2368"/>
+ <arg name="gps_imu" value="true" />
</include>
</group>
diff --git a/backpack_pkg/launch/display.rviz b/backpack_pkg/launch/display.rviz
index 306fe9937df74b90684d29f183b15734c3d90106..5d0c7b3a3cadc383dfde0aaf3ef23b83d1011eb6 100644
--- a/backpack_pkg/launch/display.rviz
+++ b/backpack_pkg/launch/display.rviz
@@ -8,6 +8,9 @@ Panels:
- /Status1
- /TF1/Frames1
- /TF1/Tree1
+ - /VLP11
+ - /VLP21
+ - /VLP2_cld_assemble1
Splitter Ratio: 0.5
Tree Height: 713
- Class: rviz/Selection
@@ -28,7 +31,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: VLP2
+ SyncSource: VLP2_cld_assemble
Preferences:
PromptSaveOnExit: true
Toolbars:
@@ -176,15 +179,57 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- - Alpha: 0.699999988079071
- Class: rviz/Map
- Color Scheme: map
- Draw Behind: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: VLP2_cld_assemble
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.0020000000949949026
+ Style: Flat Squares
+ Topic: /vlp2_cloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 25; 255; 0
Enabled: true
- Name: Map
- Topic: /map
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Queue Size: 10
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic: /trajectory
Unreliable: false
- Use Timestamp: true
Value: true
Enabled: true
Global Options:
@@ -214,7 +259,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 7.119112491607666
+ Distance: 11.369099617004395
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -230,9 +275,9 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.17479778826236725
+ Pitch: 0.014797002077102661
Target Frame: <Fixed Frame>
- Yaw: 4.379927158355713
+ Yaw: 4.294940948486328
Saved: ~
Window Geometry:
Displays:
diff --git a/backpack_pkg/launch/gps_velo_pub.launch b/backpack_pkg/launch/gps_velo_pub.launch
new file mode 100644
index 0000000000000000000000000000000000000000..839e03fc1442416911643ff19058873fee6c30ca
--- /dev/null
+++ b/backpack_pkg/launch/gps_velo_pub.launch
@@ -0,0 +1,48 @@
+<launch>
+
+ <!-- Adjusted from default nmea_serial_driver.launch file +++ in development +++ -->
+
+<!-- in devel
+ <arg name="port" default="/dev/ttyS1" />
+ <arg name="baud" default="19200" />
+ <arg name="frame_id" default="gps_velo" />
+ <arg name="use_GNSS_time" default="False" />
+ <arg name="time_ref_source" default="gps_velo" />
+ <arg name="useRMC" default="False" />
+
+ <node name="nmea_serial_driver_node" pkg="nmea_navsat_driver" type="nmea_serial_driver" output="screen">
+ <param name="port" value="$(arg port)"/>
+ <param name="baud" value="$(arg baud)" />
+ <param name="frame_id" value="$(arg frame_id)" />
+ <param name="use_GNSS_time" value="$(arg use_GNSS_time)" />
+ <param name="time_ref_source" value="$(arg time_ref_source)" />
+ <param name="useRMC" value="$(arg useRMC)" />
+ </node>
+-->
+
+<!-- works, after adjusting parser.py in nmea_navsat_driver -->
+ <arg name="time_ref_source" default="/vlp1/gpsimu_driver/gpstime" />
+ <arg name="useRMC" default="True" />
+
+
+ <node name="nmea_topic_driver" pkg="nmea_navsat_driver" type="nmea_topic_driver" output="screen">
+ <param name="time_ref_source" value="$(arg time_ref_source)" />
+ <param name="useRMC" value="$(arg useRMC)" />
+ </node>
+
+
+<!-- in devel
+ <arg name="ip" default="192.168.1.201"/>
+ <arg name="port" default="2368" />
+ <arg name="timeout" default="10" />
+
+
+ <node name="nmea_socket_driver" pkg="nmea_navsat_driver" type="nmea_socket_driver" output="screen">
+ <param name="ip" value="$(arg ip)" />
+ <param name="port" value="$(arg port)" />
+ <param name="timeout" value="$(arg timeout)" />
+ </node>
+
+-->
+</launch>
+
diff --git a/backpack_pkg/launch/in_develpment/gps4navsat.py b/backpack_pkg/launch/in_develpment/gps4navsat.py
new file mode 100644
index 0000000000000000000000000000000000000000..0947e4eaee73fc14d0302f2ff327f27d645c86d6
--- /dev/null
+++ b/backpack_pkg/launch/in_develpment/gps4navsat.py
@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+#sources: gps_wifi.py from MicaSense integration guides
+# ROS Guides
+
+import requests
+import rospy
+from nmea_msgs.msg import Sentence
+from sensor_msgs.msg import NavSatFix
+
+class PostGPS():
+
+ def __init__(self, laser_topic="/vlp1/gpsimu_driver/nmea_sentence"):
+
+ self.laser_topic = laser_topic
+ self._check_laser_ready()
+ laser_sub = rospy.Subscriber(self.laser_topic, Sentence, self.laser_callback)
+ self.rate = 0.5
+
+ def _check_laser_ready(self):
+ laser_msg = None
+ rospy.logdebug("Waiting for "+str(self.laser_topic)+" to be READY...")
+ while laser_msg is None and not rospy.is_shutdown():
+ try:
+ laser_msg = rospy.wait_for_message(self.laser_topic, Sentence, timeout=1.0)
+ rospy.logdebug("Current "+str(self.laser_topic)+"READY=>")
+ rospy.loginfo("receiving GPS information")
+ except:
+ rospy.logerr("Current "+str(self.laser_topic)+" not ready yet, retrying for getting GPS")
+
+ self.laser_sentence = laser_msg.sentence
+
+ def laser_callback(self, msg): #extract GPS information from Laser and convert NMEA Sentence to single information
+
+ '''
+ DESCRIPTION OF NMEA SENTENCE VALUES WITH EXAMPLE, only using latitude, longitude and time
+ type: $GPRMC Recommended Minimum sentence
+ time: 123519 Fix taken at 12:35:19 UTC
+ status: A Receiver status: A = Active, V = Void
+ lat: 4807.038,N Latitude 48 deg 07.038' N
+ n_s: North / South direction
+ long: 01131.000,E Longitude 11 deg 31.000' E
+ e_w: East / West direction
+ speed: 022.4 Speed over the ground (knots)
+ course: 084.4 Track made good (degrees True)
+ date: 230394 23rd of March 1994
+ magnetic_var_num and magnetic_var: 003.1,W Magnetic Variation
+ rest: *6A * followed by 2-byte Checksum
+ '''
+
+ self.laser_sentence = msg.sentence
+ type, time, status, latitude, n_s, longitude, e_w, speed, course, date, magnetic_var_num, magnetic_var, rest = self.laser_sentence.split(',')
+ self.latitude, self.longitude, self.time = latitude, longitude, time
+
+ # Sleep for a while before publishing new messages. Division is so rate != period.
+ if self.rate:
+ rospy.sleep(1/self.rate)
+ else:
+ rospy.sleep(0.5)
+
+
+ #rospy.logout("Lat "+self.lat+" Long "+self.long+" Time "+time)
+
+
+ def convert2gpsfix(self): # Convert NMEA Sentence so GPS fix, in order to implement the information to navsat_transform_node
+ #explanation: https://support.micasense.com/hc/en-us/articles/360044198153
+ rospy.loginfo("Starting publishing to camera...")
+ capture_arg = "{“use_post_capture_state”:true}"
+ capture_data = requests.post("http://192.168.10.254/capture", json=capture_arg)
+ rospy.loginfo("'http://192.168.10.254/capture' was send")
+
+ state_params = {"latitude":self.latitude,
+ "longitude":self.longitude,
+ #"altitude":imu_alt,
+ #"utc_time":"2019-12-02T12:34:56.789",
+ #"camera_phi":imu_phi,"camera_theta":imu_theta,"camera_psi":imu_psi
+ }
+ capture_data = requests.post("http://192.168.10.254/capture_state", json=state_params)
+ print(capture_data.json())
+ rospy.loginfo("'192.168.10.254/capture_state' was send")
+
+ rospy.loginfo("GPS was send to camera successfully")
+
+
+if __name__ == "__main__":
+ rospy.init_node('post_gps')
+ try:
+ post = PostGPS()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
+
+
+
diff --git a/backpack_pkg/launch/in_develpment/odom_to_path.py b/backpack_pkg/launch/in_develpment/odom_to_path.py
new file mode 100755
index 0000000000000000000000000000000000000000..b37b7a65d0e7fd16b2d3186ef0de322e971dacf7
--- /dev/null
+++ b/backpack_pkg/launch/in_develpment/odom_to_path.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+
+# source: https://answers.ros.org/question/278616/how-to-create-a-publisher-about-trajectory-path-then-show-it-in-rviz/
+import rospy
+
+from nav_msgs.msg import Path
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import PoseStamped
+
+path = Path()
+
+def odom_cb(data):
+ global path
+ path.header = data.header
+ pose = PoseStamped()
+ pose.header = data.header
+ pose.pose = data.pose.pose
+ path.poses.append(pose)
+ path_pub.publish(path)
+
+rospy.init_node('path_node')
+
+odom_sub = rospy.Subscriber('/odometry/filtered', Odometry, odom_cb)
+path_pub = rospy.Publisher('/path', Path, queue_size=10)
+
+if __name__ == '__main__':
+ rospy.spin()
diff --git a/backpack_pkg/launch/in_develpment/post_gps_2.py b/backpack_pkg/launch/in_develpment/post_gps_2.py
new file mode 100644
index 0000000000000000000000000000000000000000..4028dea715c30c14e2d7ee6ca10b74af7eae49c1
--- /dev/null
+++ b/backpack_pkg/launch/in_develpment/post_gps_2.py
@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+#sources: gps_wifi.py from MicaSense integration guides
+# ROS Guides
+
+import requests
+import rospy
+from nmea_msgs.msg import Sentence
+
+
+class PostGPS():
+
+ def __init__(self, laser_topic="/vlp1/gpsimu_driver/nmea_sentence"):
+
+ self.laser_topic = laser_topic
+ self._check_laser_ready()
+ laser_sub = rospy.Subscriber(self.laser_topic, Sentence, self.laser_callback)
+ self.rate = 0.5
+
+ def _check_laser_ready(self):
+ laser_msg = None
+ rospy.logdebug("Waiting for "+str(self.laser_topic)+" to be READY...")
+ while laser_msg is None and not rospy.is_shutdown():
+ try:
+ laser_msg = rospy.wait_for_message(self.laser_topic, Sentence, timeout=1.0)
+ rospy.logdebug("Current "+str(self.laser_topic)+"READY=>")
+ rospy.loginfo("receiving GPS information")
+ except:
+ rospy.logerr("Current "+str(self.laser_topic)+" not ready yet, retrying for getting GPS")
+
+ self.laser_sentence = laser_msg.sentence
+
+ def laser_callback(self, msg): #extract GPS information from Laser and convert NMEA Sentence to single information
+
+ '''
+ DESCRIPTION OF NMEA SENTENCE VALUES WITH EXAMPLE, only using latitude, longitude and time
+ type: $GPRMC Recommended Minimum sentence
+ time: 123519 Fix taken at 12:35:19 UTC
+ status: A Receiver status: A = Active, V = Void
+ lat: 4807.038,N Latitude 48 deg 07.038' N
+ n_s: North / South direction
+ long: 01131.000,E Longitude 11 deg 31.000' E
+ e_w: East / West direction
+ speed: 022.4 Speed over the ground (knots)
+ course: 084.4 Track made good (degrees True)
+ date: 230394 23rd of March 1994
+ magnetic_var_num and magnetic_var: 003.1,W Magnetic Variation
+ rest: *6A * followed by 2-byte Checksum
+ '''
+
+ self.laser_sentence = msg.sentence
+ type, time, status, latitude, n_s, longitude, e_w, speed, course, date, magnetic_var_num, magnetic_var, rest = self.laser_sentence.split(',')
+ self.latitude, self.longitude, self.time = latitude, longitude, time
+
+ # Sleep for a while before publishing new messages. Division is so rate != period.
+ if self.rate:
+ rospy.sleep(1/self.rate)
+ else:
+ rospy.sleep(0.5)
+
+
+ #rospy.logout("Lat "+self.lat+" Long "+self.long+" Time "+time)
+
+
+ def post_state(self): # Send Lat and Long Information from Laser to Camera via http
+ #explanation: https://support.micasense.com/hc/en-us/articles/360044198153
+ rospy.loginfo("Starting publishing to camera...")
+ capture_arg = "{“use_post_capture_state”:true}"
+ capture_data = requests.post("http://192.168.10.254/capture", json=capture_arg)
+ rospy.loginfo("'http://192.168.10.254/capture' was send")
+
+ state_params = {"latitude":self.latitude,
+ "longitude":self.longitude,
+ #"altitude":imu_alt,
+ #"utc_time":"2019-12-02T12:34:56.789",
+ #"camera_phi":imu_phi,"camera_theta":imu_theta,"camera_psi":imu_psi
+ }
+ capture_data = requests.post("http://192.168.10.254/capture_state", json=state_params)
+ print(capture_data.json())
+ rospy.loginfo("'192.168.10.254/capture_state' was send")
+
+ rospy.loginfo("GPS was send to camera successfully")
+
+
+if __name__ == "__main__":
+ rospy.init_node('post_gps')
+ try:
+ post = PostGPS()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
+
+
+
diff --git a/backpack_pkg/launch/laser_assemble.launch b/backpack_pkg/launch/laser_assemble.launch
new file mode 100644
index 0000000000000000000000000000000000000000..56a66639c43d951f3e67a2dad3f4e37f41b1971c
--- /dev/null
+++ b/backpack_pkg/launch/laser_assemble.launch
@@ -0,0 +1,13 @@
+<!-- Adapted from example on laser_assembler ROS doc -->
+
+<launch>
+
+ <node type="point_cloud2_assembler" pkg="laser_assembler" name="vlp2_assembler">
+ <remap from="cloud" to="/vlp2/velodyne_points"/>
+ <param name="max_clouds" type="int" value="200" />
+ <param name="fixed_frame" type="string" value="map" />
+ </node>
+
+ <node pkg="backpack_pkg" type="assembler.py" name="assemble_service" />
+
+</launch>
diff --git a/backpack_pkg/launch/odom_to_path.py b/backpack_pkg/launch/odom_to_path.py
index 158e9a44534afd982ab4ac3a6378b0c06bdd4a20..b37b7a65d0e7fd16b2d3186ef0de322e971dacf7 100755
--- a/backpack_pkg/launch/odom_to_path.py
+++ b/backpack_pkg/launch/odom_to_path.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
# source: https://answers.ros.org/question/278616/how-to-create-a-publisher-about-trajectory-path-then-show-it-in-rviz/
import rospy
@@ -20,7 +20,7 @@ def odom_cb(data):
rospy.init_node('path_node')
-odom_sub = rospy.Subscriber('/odometry/filtered_map', Odometry, odom_cb)
+odom_sub = rospy.Subscriber('/odometry/filtered', Odometry, odom_cb)
path_pub = rospy.Publisher('/path', Path, queue_size=10)
if __name__ == '__main__':
diff --git a/backpack_pkg/launch/post_gps_2.py b/backpack_pkg/launch/post_gps_2.py
new file mode 100644
index 0000000000000000000000000000000000000000..7d1413e30fc26070d00f08dca2c0c2fac76e286b
--- /dev/null
+++ b/backpack_pkg/launch/post_gps_2.py
@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+#sources: gps_wifi.py from MicaSense integration guides
+# ROS Guides
+
+import requests
+import rospy
+from nmea_msgs.msg import Sentence
+
+
+class PostGPS():
+
+ def __init__(self, laser_topic="/lidar/gpsimu_driver/nmea_sentence"):
+
+ self.laser_topic = laser_topic
+ self._check_laser_ready()
+ laser_sub = rospy.Subscriber(self.laser_topic, Sentence, self.laser_callback)
+ self.rate = 0.5
+
+ def _check_laser_ready(self):
+ laser_msg = None
+ rospy.logdebug("Waiting for "+str(self.laser_topic)+" to be READY...")
+ while laser_msg is None and not rospy.is_shutdown():
+ try:
+ laser_msg = rospy.wait_for_message(self.laser_topic, Sentence, timeout=1.0)
+ rospy.logdebug("Current "+str(self.laser_topic)+"READY=>")
+ rospy.loginfo("receiving GPS information")
+ except:
+ rospy.logerr("Current "+str(self.laser_topic)+" not ready yet, retrying for getting GPS")
+
+ self.laser_sentence = laser_msg.sentence
+
+ def laser_callback(self, msg): #extract GPS information from Laser and convert NMEA Sentence to single information
+
+ '''
+ DESCRIPTION OF NMEA SENTENCE VALUES WITH EXAMPLE, only using latitude, longitude and time
+ type: $GPRMC Recommended Minimum sentence
+ time: 123519 Fix taken at 12:35:19 UTC
+ status: A Receiver status: A = Active, V = Void
+ lat: 4807.038,N Latitude 48 deg 07.038' N
+ n_s: North / South direction
+ long: 01131.000,E Longitude 11 deg 31.000' E
+ e_w: East / West direction
+ speed: 022.4 Speed over the ground (knots)
+ course: 084.4 Track made good (degrees True)
+ date: 230394 23rd of March 1994
+ magnetic_var_num and magnetic_var: 003.1,W Magnetic Variation
+ rest: *6A * followed by 2-byte Checksum
+ '''
+
+ self.laser_sentence = msg.sentence
+ type, time, status, latitude, n_s, longitude, e_w, speed, course, date, magnetic_var_num, magnetic_var, rest = self.laser_sentence.split(',')
+ self.latitude, self.longitude, self.time = latitude, longitude, time
+
+ # Sleep for a while before publishing new messages. Division is so rate != period.
+ if self.rate:
+ rospy.sleep(1/self.rate)
+ else:
+ rospy.sleep(0.5)
+
+
+ #rospy.logout("Lat "+self.lat+" Long "+self.long+" Time "+time)
+
+
+ def post_state(self): # Send Lat and Long Information from Laser to Camera via http
+ #explanation: https://support.micasense.com/hc/en-us/articles/360044198153
+ rospy.loginfo("Starting publishing to camera...")
+ capture_arg = "{“use_post_capture_state”:true}"
+ capture_data = requests.post("http://192.168.10.254/capture", json=capture_arg)
+ rospy.loginfo("'http://192.168.10.254/capture' was send")
+
+ state_params = {"latitude":self.latitude,
+ "longitude":self.longitude,
+ #"altitude":imu_alt,
+ #"utc_time":"2019-12-02T12:34:56.789",
+ #"camera_phi":imu_phi,"camera_theta":imu_theta,"camera_psi":imu_psi
+ }
+ capture_data = requests.post("http://192.168.10.254/capture_state", json=state_params)
+ print(capture_data.json())
+ rospy.loginfo("'192.168.10.254/capture_state' was send")
+
+ rospy.loginfo("GPS was send to camera successfully")
+
+
+if __name__ == "__main__":
+ rospy.init_node('post_gps')
+ try:
+ post = PostGPS()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
+
+
+
diff --git a/backpack_pkg/launch/test_tf.txt b/backpack_pkg/launch/test_tf.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/backpack_pkg/launch/velo_slam.launch b/backpack_pkg/launch/velo_slam.launch
index 65bb53d44841896f06556d208ea432fbce7ebcdd..0c568a9001345e4576674f6ea4a4c63a370036cb 100644
--- a/backpack_pkg/launch/velo_slam.launch
+++ b/backpack_pkg/launch/velo_slam.launch
@@ -17,12 +17,12 @@
<param name="laser_z_min_value" value = "-0.5"/>
<param name="laser_z_max_value" value = "3.0"/>
- <param name="laser_max_dist" value = "200.0"/>
+ <param name="laser_max_dist" value = "15.0"/>
<param name="update_factor_free" value="0.3"/>
- <param name="map_resolution" value="0.05"/>
+ <param name="map_resolution" value="0.5"/>
<param name="map_size" value="1024"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5"/>
@@ -36,7 +36,9 @@
<param name="hector_mapping/pub_map_odom_transform" value="false"/>
<node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 map odom 100" />
-
+ <include file="$(find hector_geotiff_launch)/launch/geotiff_mapper.launch" >
+ <arg name="map_file_path" value="$(find backpack_pkg)/maps" />
+ </include>
<!--node pkg="map_server" type="map_saver" name="save_map" args="-f map_from_slam" Needs to be run seperatly in a new terminal with rosrun map_server map_saver -f my_map-->
diff --git a/backpack_pkg/params/ekf_navsat.yaml b/backpack_pkg/params/ekf_navsat.yaml
index f2ad156d4079f496da55f74259fdbf951bc1d58e..3ac07b4d15ca37c121d1a321422056339d77af3c 100644
--- a/backpack_pkg/params/ekf_navsat.yaml
+++ b/backpack_pkg/params/ekf_navsat.yaml
@@ -15,8 +15,8 @@ ekf_se_odom:
world_frame: odom
pose0: poseupdate
- pose0_config: [true, true, true,
- true, true, true,
+ pose0_config: [true, true, false,
+ false, false, false,
false, false, false,
false, false, false,
false, false, false]
@@ -74,10 +74,10 @@ ekf_se_map:
base_link_frame: base_link
world_frame: map
- odom0: odometry/gps
- odom0_config: [false, false, false,
+ odom0: /odometry/gps #published by navsat_transform_node
+ odom0_config: [true, true, false,
+ false, false, false,
false, false, false,
- true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 10
@@ -85,6 +85,16 @@ ekf_se_map:
odom0_differential: false
odom0_relative: false
+ #odom1: /vlp1/odometry/gps #published by navsat_transform_node
+ #odom1_config: [true, true, false,
+ # false, false, false,
+ # false, false, false,
+ # false, false, false,
+ # false, false, false]
+ #odom1_queue_size: 10
+ #odom1_nodelay: true
+ #odom1_differential: false
+ #odom1_relative: false
#twist0: filter/twist
#twist0_config: [false, false, false,
# false, false, false,
@@ -95,10 +105,10 @@ ekf_se_map:
imu0: imu/data
imu0_config: [false, false, false,
- false, false, false,
- true, true, true,
true, true, true,
- false, false, false]
+ false, false, false,
+ true, true, true,
+ true, true, true]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false