import rospy
from random import shuffle
import numpy as np
import cv2
import navigator_tools
import tf.transformations as trns
from navigator_tools import fprint as _fprint
from navigator_msgs.srv import ObjectDBQuery
from navigator_msgs.msg import PerceptionObject
from nav_msgs.msg import OccupancyGrid, Odometry
from std_srvs.srv import Trigger
fprint = lambda *args, **kwargs: _fprint(time='', title='SIM', *args, **kwargs)
class DoOdom(object):
"""Republish odom for lqrrt"""
def __init__(self, rand_size):
self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
self.odom = None
self.carrot_sub = rospy.Subscriber("/lqrrt/ref", Odometry,
self.set_odom)
fprint("Shaking hands and taking names.")
rospy.sleep(1)
# We need to publish an inital odom message for lqrrt
start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
#!/usr/bin/env python
import rospy
import actionlib
from navigator_alarm import AlarmListener
from lqrrt_ros.msg import MoveAction, MoveGoal
from navigator_msgs.srv import WrenchSelect, WrenchSelectRequest
from navigator_tools import fprint as _fprint
fprint = lambda *args, **kwargs: _fprint(
title="STATION_HOLDER", time="", *args, **kwargs)
class StationHoldListener():
def __init__(self):
fprint("Starting station hold listener...")
self.client = actionlib.SimpleActionClient('/move_to', MoveAction)
fprint("Waiting for action server...")
self.client.wait_for_server()
self.change_wrench = rospy.ServiceProxy("/change_wrench", WrenchSelect)
self.change_wrench.wait_for_service()
fprint("Creating alarm listener...")
self.station_hold_alarm = AlarmListener(alarm_name='station_hold',
callback_funct=self.handle)
fprint("Ready to go!", msg_color="green")
def handle(self, alarm):
import numpy as np
import rospy
import actionlib
import tf.transformations as trns
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import PoseStamped
from lqrrt_ros.msg import MoveAction, MoveFeedback, MoveResult
import navigator_tools
from navigator_tools import fprint as _fprint
from behaviors import params
fprint = lambda *args, **kwargs: _fprint(title="FAKE_ACTION_SERVER", time="", *args, **kwargs)
class FakeActionServer(object):
def __init__(self):
self.goal_pose_pub = rospy.Publisher("/lqrrt/goal", PoseStamped, queue_size=3)
# Parameters to simulate lqrrt
self.blind = False
self.ogrid = None
set_ogrid = lambda msg: setattr(self, "ogrid", msg)
rospy.Subscriber("/ogrid_master", OccupancyGrid, set_ogrid)
self.move_server = actionlib.SimpleActionServer("/move_to", MoveAction, execute_cb=self.move_cb, auto_start=False)
self.move_server.start()
rospy.sleep(.1)
from navigator_tools import fprint as _fprint
import cv2
import numpy as np
from copy import deepcopy
from dynamic_reconfigure.server import Server
from dynamic_reconfigure.client import Client
from navigator_msg_multiplexer.cfg import OgridConfig
from nav_msgs.msg import OccupancyGrid, MapMetaData, Odometry
from behaviors import params
# Wow what a concept
fprint = lambda *args, **kwargs: _fprint(title="OGRID_ARB", *args, **kwargs)
def make_ogrid_transform(ogrid):
"""
Returns a matrix that transforms a point in ENU to this ogrid.
Invert the result to get ogrid -> ENU.
"""
resolution = ogrid.info.resolution
origin = navigator_tools.pose_to_numpy(ogrid.info.origin)[0]
# Transforms points from ENU to ogrid frame coordinates
t = np.array([[1 / resolution, 0, -origin[0] / resolution],
[0, 1 / resolution, -origin[1] / resolution],
[0, 0, 1]])
return t
import rospy
from random import shuffle
import numpy as np
import cv2
import navigator_tools
import tf.transformations as trns
from navigator_tools import fprint as _fprint
from navigator_msgs.srv import ObjectDBQuery
from navigator_msgs.msg import PerceptionObject
from nav_msgs.msg import OccupancyGrid, Odometry
from std_srvs.srv import Trigger
fprint = lambda *args, **kwargs: _fprint(time='', title='SIM',*args, **kwargs)
class DoOdom(object):
"""Republish odom for lqrrt"""
def __init__(self, rand_size):
self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
self.odom = None
self.carrot_sub = rospy.Subscriber("/lqrrt/ref", Odometry, self.set_odom)
fprint("Shaking hands and taking names.")
rospy.sleep(1)
# We need to publish an inital odom message for lqrrt
start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
start_pos = np.append(np.random.uniform(rand_size, size=(2)), 1)
start_pose = navigator_tools.numpy_quat_pair_to_pose(start_pos, start_ori)
from navigator_tools import fprint as _fprint
from std_srvs.srv import Trigger
import cv2
import numpy as np
from copy import deepcopy
from dynamic_reconfigure.server import Server
from dynamic_reconfigure.client import Client
from navigator_msg_multiplexer.cfg import OgridConfig
from nav_msgs.msg import OccupancyGrid, MapMetaData, Odometry
from behaviors import params
# Wow what a concept
fprint = lambda *args, **kwargs: _fprint(title="OGRID_ARB", *args, **kwargs)
def make_ogrid_transform(ogrid):
"""
Returns a matrix that transforms a point in ENU to this ogrid.
Invert the result to get ogrid -> ENU.
"""
resolution = ogrid.info.resolution
origin = navigator_tools.pose_to_numpy(ogrid.info.origin)[0]
# Transforms points from ENU to ogrid frame coordinates
t = np.array([[1 / resolution, 0, -origin[0] / resolution],
[0, 1 / resolution, -origin[1] / resolution], [0, 0, 1]])
return t
from navigator_msgs.srv import ObjectDBQuery, ObjectDBQueryRequest, ColorRequest, ColorRequestResponse
from navigator_msgs.msg import ColoramaDebug
from sensor_msgs.msg import CameraInfo, Image
from nav_msgs.msg import Odometry
from cv_bridge import CvBridge, CvBridgeError
from image_geometry import PinholeCameraModel
import navigator_tools
from navigator_tools import fprint as _fprint
camera_root = "/stereo/right" # /camera_root/root
ros_t = lambda t: rospy.Duration(t)
fprint = lambda *args, **kwargs: _fprint(title="COLORAMA", time="", *args, **kwargs)
p2np = navigator_tools.point_to_numpy
class ImageHolder(object):
@classmethod
def from_msg(cls, msg):
time = msg.header.stamp
fprint("Got image! {}".format(time.to_sec()))
try:
image = CvBridge().imgmsg_to_cv2(msg)
return cls(image, time)
except CvBridgeError, e:
# Intentionally absorb CvBridge Errors
rospy.logerr(e)
return cls()
#!/usr/bin/env python
import rospy
import threading
import serial
from std_msgs.msg import String
from std_msgs.msg import Header
from navigator_tools import thread_lock
from navigator_tools import fprint as _fprint
from navigator_alarm import AlarmBroadcaster, AlarmListener
from navigator_msgs.msg import KillStatus
fprint = lambda *args, **kwargs: _fprint(time='', *args, **kwargs)
lock = threading.Lock()
class KillInterface(object):
"""
This handles the comms node between ROS and kill/status embedded board.
There are two things running here:
1. From ROS: Check current operation mode of the boat and tell that to the light
2. From BASE: Check the current kill status from the other sources
"""
def __init__(
self,
port="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0",
baud=9600):
self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.25)
self.ser.flush()
#!/usr/bin/env python
import rospy
import actionlib
from navigator_alarm import AlarmListener
from lqrrt_ros.msg import MoveAction, MoveGoal
from navigator_msgs.srv import WrenchSelect, WrenchSelectRequest
from navigator_tools import fprint as _fprint
fprint = lambda *args, **kwargs: _fprint(title="STATION_HOLDER", time="", *args, **kwargs)
class StationHoldListener():
def __init__(self):
fprint("Starting station hold listener...")
self.client = actionlib.SimpleActionClient('/move_to', MoveAction)
fprint("Waiting for action server...")
self.client.wait_for_server()
self.change_wrench = rospy.ServiceProxy("/change_wrench", WrenchSelect)
self.change_wrench.wait_for_service()
fprint("Creating alarm listener...")
self.station_hold_alarm = AlarmListener(alarm_name='station_hold', callback_funct=self.handle)
fprint("Ready to go!", msg_color="green")
def handle(self, alarm):
if alarm.clear:
# Not of intrest since it's being cleared
return
import tf
import sensor_msgs.point_cloud2 as pc2
from navigator_msgs.srv import ObjectDBQuery, ObjectDBQueryRequest, VisionRequest, VisionRequestResponse
from sensor_msgs.msg import CameraInfo, Image
from cv_bridge import CvBridge, CvBridgeError
from image_geometry import PinholeCameraModel
import navigator_tools
from navigator_tools import fprint as _fprint
camera_root = "/stereo/right" # /camera_root/root
rospy.init_node("database_colorer")
ros_t = lambda t: rospy.Duration(t)
fprint = lambda *args, **kwargs: _fprint(
title="COLORAMA", time="", *args, **kwargs)
class ImageHolder(object):
@classmethod
def from_msg(cls, msg):
time = msg.header.stamp
try:
image = CvBridge().imgmsg_to_cv2(msg)
return cls(image, time)
except CvBridgeError, e:
# Intentionally absorb CvBridge Errors
rospy.logerr(e)
return cls()
#!/usr/bin/env python
import rospy
import threading
import serial
from std_msgs.msg import String
from navigator_tools import thread_lock
from navigator_tools import fprint as _fprint
from navigator_alarm import AlarmBroadcaster, AlarmListener
from navigator_msgs.msg import KillStatus
fprint = lambda *args, **kwargs: _fprint(time='', *args, **kwargs)
lock = threading.Lock()
class KillInterface(object):
"""
This handles the comms node between ROS and kill/status embedded board.
There are two things running here:
1. From ROS: Check current operation mode of the boat and tell that to the light
2. From BASE: Check the current kill status from the other sources
"""
def __init__(self, port="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0", baud=9600):
self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.25)
self.ser.flush()
self.killstatus_pub = rospy.Publisher("/killstatus", KillStatus, queue_size=1)
