API Details¶
The following sections outline the details of the ROS1 Noetic API that can be used with OutdoorNav Software.
Platform API¶
The Platform API provides topics to communicate with the hardware platform (eg. sensor data, command velocity, tfs, etc…). This API can be used by autonomy software packages to interface with the hardware platform.
Topics Published by UGV¶
The topics in this section are published by the UGV. If available, they are used by the OutdoorNav software.
tf¶
Message Type: tf2_msgs/TFMessage
Usage: The tf topic represents the relationship between coordinate frames (sensors frame and localization frame). See http://wiki.ros.org/tf for more information.
tf_static¶
Message Type: tf2_msgs/TFMessage
Usage: This is similar to the necessary tf topic previously described, but it only needs to be published and latched once for transforms that are static and do not change. See http://wiki.ros.org/tf for more information.
<platform>_velocity_controller/odom¶
Message Type: nav_msgs/Odometry
Usage: This is the odometry provided by the platform control system, usually a velocity controller. Replace <platform> with the name of your robot (ie. jackal/husky/warthog). If a third-party vehicle, then you should have your own ROS odometry topic that is being used as input to OutdoorNav.
extra_imu/imu/data¶
Message Type: sensor_msgs/Imu
Usage: This is the data published from the IMU. OutdoorNav requires the data published by the gyroscope of the imu for localization. The transform frame in which this topic is published must be available in the tf tree of the UGV, i.e., /tf_static topic.
rtk_heading¶
Message Type: sensor_msgs/Imu
Usage: Accurate RTK heading published from the dual antenna GPS system on the UGV. Only the yaw information (rotation around z axis) is used by the OutdoorNav localization module. The transform frame in which this topic is published must be available in the tf tree of the UGV, i.e., /tf_static topic.
clock¶
Message Type: rosgraph_msgs/Clock
Usage: The current time of the ROS system running on the platform. This must be of sufficient frequency, but excessive publishing frequency (greater than 100 Hz) may have an adverse effect.
front/scan¶
Message Type: sensor_msgs/LaserScan
Usage: The laser scan data from the front 2D lidar, that the OutdoorNav software will use for obstacle detection/avoidance. If only one 2D lidar is on your UGV, then its data will be published to this topic. The OutdoorNav software expects this topics TF to be relative to “front_laser”.
rear/scan¶
Message Type: sensor_msgs/LaserScan
Usage: The laser scan data from the rear 2D lidar, that the OutdoorNav software will use for obstacle detection/avoidance. This will be published only if a second 2D lidar is on your UGV (ie. one at the front and the other at ther rear). The OutdoorNav software expects this topics TF to be relative to “rear_laser”.
velodyne_points¶
Message Type: Type: sensor_msgs/PointCloud2
Usage: Pointcloud data published by 3D lidar, that the OutdoorNav software will use for obstacle detection/avoidance. If only one 3D lidar is on your UGV, then its data will be published to this topic. The OutdoorNav software expects this topics TF to be relative to “velodyne”.
rear/velodyne_points¶
Message Type: Type: sensor_msgs/PointCloud2
Usage: Pointcloud data published by 3D lidar, that the OutdoorNav software will use for obstacle detection/avoidance. This will be published only if a second 2D lidar is on your UGV (ie. one at the front and the other at ther rear). The OutdoorNav software expects this topics TF to be relative to “rear_velodyne”.
microhard_snmp¶
Message Type: microhard_msg
Usage: If a microhard wireless system is used for wifi communication, this topic can be used to obtain the wifi connection information between the platform and the Base Station (or other connected network). This includes signal strength, transmit power, rssi, etc…
Autonomy API¶
The Autonomy provides some relevant topics and services suitable for controlling and monitoring the robot as it is performing autonomous navigation.
Definitions¶
Message Definitions¶
microhard_msg¶
Header header
string ssid
string wifi_mode
int64 snr
int64 rssi
int64 tx_power
int8 connected_bool
string message
Standard ROS Messages¶
Advanced Usage¶
Tasks¶
Tasks are called after the UGV reaches a mission’s goal point. A Task is a ROS Action call from its specified namespace. An action is a function in a node which can be called remotely, that terminates when a specified goal is reached. This can be used to execute a more complex and useful function at a goal point. Tasks will block the execution of the next mission but can be cancelled while they are in the process of being executed. For instance, if the Task is for the UGV to move its robotics arm to a point and pick up an object, the UGV will remain in place while the Task is being executed, until it has completed or has been cancelled. Once the action is complete, OutdoorNav will resume execution of the next goal in the defined sequence.
Below is an example of a simple task in python which prints the GPS coordinates whenever the Task is called.
from std_srvs.srv import *
from sensor_msgs.msg import NavSatFix
import rospy
lat = None
lon = None
import rospy
import actionlib
from sensor_msgs.msg import NavSatFix
import cpr_gps_navigation_msgs.msg
class RecordGPS(object):
_result = cpr_gps_navigation_msgs.msg.UITaskResult()
lat = None
lon = None
def __init__(self, name):
self.sub = rospy.Subscriber("/navsat/upgrade/fix", NavSatFix, self.fix_sub)
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name, cpr_gps_navigation_msgs.msg.UITaskAction, execute_cb=self.handle_record_gps, auto_start=False)
self._as.start()
def handle_record_gps(self, goal):
rospy.loginfo("Latitude: %f Longtitude: %f"%(self.lat, self.lon))
with open('goal_point_positions.txt', 'a') as the_file:
the_file.write("Latitude: %f Longtitude: %f \n"%(self.lat, self.lon))
the_file.close()
self._result.success = True
self._as.set_succeeded(self._result)
def fix_sub(self, msg):
self.lat = msg.latitude
self.lon = msg.longitude
if __name__ == '__main__':
rospy.init_node('record_gps', anonymous=False)
OBJ = RecordGPS(rospy.get_name())
rospy.loginfo('record gps server started.')
rospy.spin()
Below is another example action task for the UGV to wait at the goal point. This Task is already included in the OutdoorNav software and is available under the /wait namespace.
import rospy
import actionlib
import cpr_gps_navigation_msgs.msg
class Wait(object):
_result = cpr_gps_navigation_msgs.msg.UITaskResult()
sleep_time = 0.1
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name, cpr_gps_navigation_msgs.msg.UITaskAction, execute_cb=self.waitCB, auto_start=False)
self._as.start()
def waitCB(self, goal):
success = True
# num_sleeps_cycles = 10.0/self.sleep_time;
num_sleeps_cycles = goal.floats[0]/self.sleep_time;
i = 0;
while i <= num_sleeps_cycles:
if self._as.is_preempt_requested():
success = False
self._result.success = success
self._as.set_preempted(self._result)
break
rospy.sleep(self.sleep_time);
i = i+1
if success:
self._result.success = success
self._as.set_succeeded(self._result)
if __name__ == '__main__':
rospy.init_node('wait', anonymous=False)
OBJ = Wait(rospy.get_name())
rospy.loginfo('Wait server started.')
rospy.spin()
For more information on providing a ROS Action see http://wiki.ros.org/actionlib and http://wiki.ros.org/actionlib/Tutorials.