SubjuGator Software Reference

Below is the reference documentation for the code on our submarine robot, SubjuGator. The following systems are relevant only to this robot, and no other robot. However, hyperlinks may link to systems on other robots.

Messages

Thrust

Attributes

• thruster_commands

class subjugator_msgs.msg.Thrust

Message type indicating commands for each thruster.

thruster_commands

The commands for the thrusters.

Type

List[ThrusterCmd]

ThrusterCmd

Attributes

• name
• thrust

class subjugator_msgs.msg.ThrusterCmd

A command for a specific thruster.

thrust

The amount of thrust for the specific thruster.

Type

float

name

The name of the thruster.

Type

str

Services

SetValve

Attributes

• actuator
• opened

class sub_actuator_board.srv.SetValveRequest

The request class for the sub_actuator_board/SetValve service.

actuator

Which actuator on the sub the message references.

Type

int

opened

Whether the valve should be opened or closed.

Type

bool

Attributes

• message
• success

class sub_actuator_board.srv.SetValveResponse

The response class for the sub_actuator_board/SetValve service.

success

The success of the operation.

Type

bool

message

If an error occurred, a message depicting the error.

Type

bool

Actuator Board

ActuatorBoard

Methods

• defon_data
• defset_valve

class sub_actuator_board.ActuatorBoard(*args, **kwargs)

Device handle for the actuator board. Because this class implements a CAN device, it inherits from the CANDeviceHandle class.

on_data(packet: ActuatorPollResponsePacket | AckPacket) → None

Process data received from board.

set_valve(req: SetValveRequest) → dict

Called when the /set_valve service is requested. Creates a message to control the valve and sends it through the inherited device handle.

Parameters

req (SetValveRequest) – Request to set the valve.

Returns

List of information which is casted into a SetValveResponse.

Return type

dict

ActuatorBoardSimulation

Attributes

• status

Methods

• defon_data

class sub_actuator_board.ActuatorBoardSimulation(*args, **kwargs)

Simulator for the communication of the actuator board.

status

The status of the valves. The keys are each of the valve IDs, and the values are the statues of whether the valves are open.

Type

Dict[int, bool]

on_data(packet: ActuatorSetPacket | ActuatorPollRequestPacket) → None

Processes data received from motherboard / other devices. For each message received, the class’ status attribute is updated if the message is asking to write, otherwise a feedback message is constructed and sent back.

ActuatorPacketId

class sub_actuator_board.ActuatorPacketId(value)

Enumerator representing each controllable actuator.

BALL_DROP = 2

The ball drop actuator. Only one actuator is used for both balls.

GRIPPER = 0

The gripper actuator.

TORPEDO_LAUNCHER = 1

The torpedo launcher actuator.

ActuatorSetPacket

Attributes

• address
• open

class sub_actuator_board.ActuatorSetPacket(address: ActuatorPacketId, open: bool)

Packet used by the actuator board to set a specific valve.

address

The actuator ID to set.

Type

ActuatorPacketId

open

Whether to open the specified actuator. True requests opening, False requests closing.

Type

bool

ActuatorPollRequestPacket

class sub_actuator_board.ActuatorPollRequestPacket

Packet used by the actuator board to request the status of all valves.

ActuatorPollResponsePacket

Attributes

• ball_drop_opened
• gripper_opened
• torpedo_launcher_opened
• values

class sub_actuator_board.ActuatorPollResponsePacket(values: int)

Packet used by the actuator board to return the status of all valves.

values

The statues of all actuators. Bits 0-3 represent the opened status of actuators 0-3.

Type

int

property ball_drop_opened: bool

Whether the ball drop is opened.

property gripper_opened: bool

Whether the gripper is opened.

property torpedo_launcher_opened: bool

Whether the torpedo launcher is opened.

Sub8 Thrust and Kill Board

ThrusterAndKillBoard

Methods

• defon_command
• defon_data
• defon_hw_kill
• defsend_heartbeat
• defset_mobo_kill
• defupdate_hw_kill

class sub8_thrust_and_kill_board.ThrusterAndKillBoard(*args, **kwargs)

Device handle for the thrust and kill board.

on_command(msg: Thrust) → None

When a thrust command message is received from the Subscriber, send the appropriate packet to the board.

Parameters

msg (Thrust) – The thrust message.

on_data(data: bytes) → None

Parse the two bytes and raise kills according to a set of specifications listed below.

Specifications of the first byte (where “asserted” is 1 and “unasserted” is 0):

First Byte	Meaning
Bit 7	Hard kill status, changed by the Hall Effect switch. If this becomes 1, the shutdown procedure begins.
Bit 6	The generalized soft kill status. Becomes 1 if any of the specific soft kill flags become 1. Returns to 0 if all kills are cleared and the thrusters are done re-initializing.
Bit 5	Soft kill flag for the Hall Effect sensor
Bit 4	Motherboard command soft kill flag.
Bit 3	Soft kill indicating heartbeat was lost. Times out after one second.
Bit 2 Bit 1 Bit 0	Reserved flags.

Specifications of the second byte (where “pressed” is 1 and “unpressed” is 0):

Second Byte	Meaning
Bit 7	Hard kill status representing the Hall Effect switch. If this becomes 0 (“on”), the hard kill in the previous byte becomes 1.
Bit 6	The soft kill status of the Hall Effect sensor. If this is “pressed” (1) then bit 5 of the previous byte becomes “unkilled” (0). Removing the magnet “unpresses” the switch.
Bit 5	Go Hall Effect switch status. “Pressed” is 1, removing the magnet “unpresses” the switch.
Bit 4	Thruster initializing status. Becomes 1 when the board is unkilling and starts powering thrusters. After the “grace period” it becomes 0 and the overall soft kill status becomes 0. The flag also becomes 0 if killed in the middle of initializing thrusters.
Bit 3 Bit 2 Bit 1 Bit 0	Reserved flags.

on_hw_kill(alarm: Alarm) → None

Update the classes’ hw-kill alarm to the latest update.

Parameters

alarm (Alarm) – The alarm message to update with.

send_heartbeat(_: TimerEvent) → None

Send a special heartbeat packet. Called by a recurring timer set upon initialization.

set_mobo_kill(req: SetBoolRequest) → SetBoolResponse

Called on service calls to /set_mobo_kill, sending the appropriate packet to the board to unassert or assert to motherboard-origin kill.

Parameters

req (SetBoolRequest) – The service request.

Returns

The service response.

Return type

SetBoolResponse

update_hw_kill(status: StatusMessage) → None

If needed, update the hw-kill alarm so it reflects the latest status from the board.

Parameters

status (StatusMessage) – The status message.

KillMessage

Attributes

• ASSERTED
• COMMAND
• HARD
• IDENTIFIER
• RESPONSE
• SOFT
• UNASSERTED
• is_asserted
• is_command
• is_hard
• is_response
• is_soft
• is_unasserted

Methods

• clsKillMessage.create_kill_message

class sub8_thrust_and_kill_board.KillMessage(identifier: int, payload: bytes)

Represents a packet sent to kill/thrust board which contains a command or response related to kill status.

Inherits from ApplicationPacket.

str(x)

Pretty-prints the class name and each of the three packet attributes.

IDENTIFIER

The packet’s identifier. Set equal to the ordinal value of “K”, or 75.

Type

int

COMMAND

Identifier representing the packet as a command packet. Equal to 67. This identifier should only be found in the first byte of the payload.

Type

int

RESPONSE

Identifier representing the packet as a response packet. Equal to 82. This identifier should only be found in the first byte of the payload.

Type

int

HARD

Identifier representing the type of kill as a hard kill. Equal to 72. This identifier is designed to be used in the second byte of the payload.

Type

int

SOFT

Identifier representing the type of kill as a soft kill. Equal to 83. This identifier is designed to be used in the second byte of the payload.

Type

int

ASSERTED

Identifier equal to 65. This identifier is meant to be used in the third byte of the payload.

Type

int

UNASSERTED

Identifier equal to 85. This identifier is meant to be used in the third byte of the payload.

Type

int

classmethod create_kill_message(command: bool = False, hard: bool = False, asserted: bool = False)

Creates a kill message containing three bytes of information, specified as parameters.

Parameters

• command (bool) – Whether to make the kill message a command. If False, then the packet will represent a response.

• hard (bool) – Whether to make the packet kill type hard. If False, then the packet will represent a soft kill.

• asserted (bool) – Whether to make the packet asserted. If False, then an unasserted packet is generated.

property is_asserted

Whether the packet represents an asserted packet.

Returns

The status of the packet’s asserteed/unasserted type.

Return type

bool

property is_command: bool

Whether the packet represents a command.

Returns

The status of the packet’s command/response type.

Return type

bool

property is_hard: bool

Whether the packet represents a hard kill.

Returns

The status of the packet’s hard/soft kill type.

Return type

bool

property is_response: bool

Whether the packet represents a response.

Returns

The status of the packet’s command/response type.

Return type

bool

property is_soft: bool

Whether the packet represents a soft kill.

Returns

The status of the packet’s hard/soft kill type.

Return type

bool

property is_unasserted

Whether the packet represents an unasserted packet.

Returns

The status of the packet’s asserteed/unasserted type.

Return type

bool

HeartbeatMessage

Attributes

• IDENTIFIER

Methods

• clsHeartbeatMessage.create

class sub8_thrust_and_kill_board.HeartbeatMessage(identifier: int, payload: bytes)

Represents the special heartbeat packet send to kill and thruster board.

Inherits from ApplicationPacket.

str(x)

Pretty-prints the class name.

IDENTIFIER

The packet identifier. Set equal to the ordinal value of “H,” or 72.

Type

int

classmethod create()

Creates a new heartbeat message to be sent.

ThrustPacket

Attributes

• IDENTIFIER
• command
• thruster_id

Methods

• clsThrustPacket.create_thrust_packet

class sub8_thrust_and_kill_board.ThrustPacket(identifier: int, payload: bytes)

Represents a command send to the thrust/kill board to set the PWM of a thruster.

Inherits from ApplicationPacket.

str(x)

Pretty-prints the class name and each of the two packet attributes, the thruster ID and the command.

IDENTIFIER

The packet identifier, equal to the ordinal value of “T,” or 84.

Type

int

property command: float

The command associated with the packet.

Returns

The associated command.

Return type

float

classmethod create_thrust_packet(thruster_id: int, command: float)

Creates a thruster packet given an ID and command.

Parameters

• thruster_id (int) – The ID of the thruster to create a packet for.

• command (float) – The command to associate with the packet.

property thruster_id: int

The thruster ID associated with the packet.

Returns

The ID.

Return type

int

ThrusterAndKillBoardSimulation

Attributes

• hard_kill_mobo
• hard_kill_plug_pulled
• hard_killed
• heartbeat_timedout
• soft_kill_mobo
• soft_kill_plug_pulled
• soft_killed

Methods

• defon_data
• defsend_updates
• defset_hard_kill
• defset_soft_kill

class sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation(*args, **kwargs)

Serial simulator for the thruster and kill board, providing services to simulate physical plug connections/disconnections.

Inherits from SimulatedCANDevice.

hard_kill_plug_pulled

Whether the hard kill was set.

Type

bool

hard_kill_mobo

Whether the motherboard experienced a hard kill request.

Type

bool

soft_kill_plug_pulled

Whether the soft kill was set.

Type

bool

soft_kill_mobo

Whether the motherboard experienced a soft kill request.

Type

bool

property hard_killed: bool

Whether the board was hard killed.

Returns

The status of the board being hard killed.

Return type

bool

property heartbeat_timedout: bool

Whether the heartbeat timed out.

Returns

The status of the heartbeat timing out.

Return type

bool

on_data(data: bytes, can_id: int) → None

Serves as the data handler for the device. Handles KillMessage, ThrustPacket, and HeartbeatMessage types.

send_updates(*args) → None

Sends data about the class in a new status message.

set_hard_kill(req: SetBoolRequest) → SetBoolResponse

Called by the /simulate_hard_kill service to set the hard kill state of the simluated device.

Parameters

req (SetBoolRequest) – The request to set the service with.

Returns

The response to the service that the operation was successful.

Return type

SetBoolResponse

set_soft_kill(req: SetBoolRequest) → SetBoolResponse

Called by the /simulate_soft_kill service to set the soft kill state of the simluated device.

Parameters

req (SetBoolRequest) – The request to set the service with.

Returns

The response to the service that the operation was successful.

Return type

SetBoolResponse

property soft_killed: bool

Whether the board was soft killed.

Returns

The status of the board being soft killed.

Return type

bool

Thruster

Attributes

• backward_calibration
• forward_calibration

Methods

• clsThruster.from_dict
• defeffort_from_thrust

class sub8_thrust_and_kill_board.Thruster(forward_calibration, backward_calibration)

Models the force (thrust) to PWM (effort) of a thruster.

forward_calibration

???

Type

???

backward_calibration

???

Type

???

effort_from_thrust(thrust: Any)

Attempts to find the effort from a particular value of thrust.

Parameters

thrust – The amount of thrust.

classmethod from_dict(data: dict[str, dict[str, Any]])

Constructs the class from a dictionary. The dictionary should be formatted as so:

{
    "calib": {
        "forward": ...,
        "backward": ...,
    }
}

Parameters

data (Dict[str, Dict[str, Any]]) – The dictionary containing the initialization info.

Sub9 Thrust and Kill Board

HeartbeatSetPacket

class sub9_thrust_and_kill_board.HeartbeatSetPacket

Heartbeat packet sent by the motherboard to the thrust/kill board.

HeartbeatReceivePacket

class sub9_thrust_and_kill_board.HeartbeatReceivePacket

Heartbeat packet sent by the thrust/kill board to the motherboard.

ThrusterId

Attributes

• BLH
• BLV
• BRH
• BRV
• FLH
• FLV
• FRH
• FRV

class sub9_thrust_and_kill_board.ThrusterId(value)

Enum to represent the thruster ID.

BLH = 4

Back left horizontal thruster.

BLV = 6

Back left vertical thruster.

BRH = 5

Back right horizontal thruster.

BRV = 7

Back right vertical thruster.

FLH = 0

Front left horizontal thruster.

FLV = 2

Front left vertical thruster.

FRH = 1

Front right horizontal thruster.

FRV = 3

Front right vertical thruster.

ThrustSetPacket

Attributes

• speed
• thruster_id

class sub9_thrust_and_kill_board.ThrustSetPacket(thruster_id: ThrusterId, speed: float)

Packet to set the speed of a specific thruster.

thruster_id

The ID of the thruster to set the speed of. The ID of the thruster corresponds to a specific thruster:

Type

ThrusterId

speed

The speed to set the thruster to.

Type

float

KillStatus

Attributes

• BATTERY_LOW
• BOARD_HEARTBEAT_LOST
• KILL_SWITCH
• MOBO_HEARTBEAT_LOST
• SOFTWARE_REQUESTED

class sub9_thrust_and_kill_board.KillStatus(value)

Enum to represent the purpose behind a kill.

BATTERY_LOW = 4

The battery is too low to continue operation.

BOARD_HEARTBEAT_LOST = 2

The motherboard reported that it stopped hearing the heartbeat from the board.

KILL_SWITCH = 3

The physical kill switch was pulled, requesting an immediate kill.

MOBO_HEARTBEAT_LOST = 1

The board reported that it stopped hearing the heartbeat from the motherboard.

SOFTWARE_REQUESTED = 0

A software user manually requested a kill on the sub.

KillSetPacket

Attributes

• set
• status

class sub9_thrust_and_kill_board.KillSetPacket(set: bool, status: KillStatus)

Packet sent by the motherboard to set/unset the kill.

set

Whether the kill is set or unset.

Type

bool

status

The reason for the kill.

Type

KillStatus

KillReceivePacket

Attributes

• set
• status

class sub9_thrust_and_kill_board.KillReceivePacket(set: bool, status: KillStatus)

Packet sent by the motherboard to set/unset the kill.

set

Whether the kill is set or unset.

Type

bool

status

The reason for the kill.

Type

KillStatus

Object Detection

SubjuGatorBuoyDetector

Attributes

• buoy_radius
• cam_model
• color_ranges
• compute_timer
• computing
• current_cloud
• data_sub
• got_cloud
• got_image
• image_pub
• image_sub
• image_time
• image_transport
• last_bump_target
• last_cloud_time
• last_draw_image
• last_image_msg
• line_added
• need_new_cloud
• nh
• rviz
• service_2d
• service_3d
• viewer
• vp1
• vp2

Functions

• funcSubjuGatorBuoyDetector
• funccloud_callback
• funccompute_loop
• funcdetermine_buoy_position
• funcget_last_image
• funcimage_callback
• funcrequest_buoy_position
• funcrequest_buoy_position_2d
• funcsegment_buoy
• func~SubjuGatorBuoyDetector

class SubjuGatorBuoyDetector

Public Functions

SubjuGatorBuoyDetector()

~SubjuGatorBuoyDetector()

void compute_loop(const ros::TimerEvent&)

void cloud_callback(const sensor_msgs::PointCloud2::ConstPtr&)

void image_callback(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &info_msg)

bool get_last_image(cv::Mat &last_image)

bool determine_buoy_position(const image_geometry::PinholeCameraModel &cam_model, const std::string &target_color, const cv::Mat &image_raw, const sub::PointCloudT::Ptr &point_cloud_raw, Eigen::Vector3f &center)

bool segment_buoy(cv::Mat &input_image, cv::Point &center, std::vector<sub::Contour> &output_contours, std::string &target_name)

bool request_buoy_position_2d(subjugator_msgs::VisionRequest2D::Request &req, subjugator_msgs::VisionRequest2D::Response &resp)

bool request_buoy_position(subjugator_msgs::VisionRequest::Request &req, subjugator_msgs::VisionRequest::Response &resp)

Public Members

int vp1

int vp2

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer

sub::RvizVisualizer rviz

ros::Timer compute_timer

ros::Subscriber data_sub

ros::NodeHandle nh

ros::ServiceServer service_2d

ros::ServiceServer service_3d

double buoy_radius

std::map<std::string, sub::Range> color_ranges

Eigen::Vector3f last_bump_target

image_transport::CameraSubscriber image_sub

image_transport::ImageTransport image_transport

image_transport::Publisher image_pub

cv::Mat last_draw_image

image_geometry::PinholeCameraModel cam_model

ros::Time image_time

ros::Time last_cloud_time

sub::PointCloudT::Ptr current_cloud

sensor_msgs::ImageConstPtr last_image_msg

bool got_cloud

bool got_image

bool line_added

bool computing

bool need_new_cloud

SubjuGatorStartGateDetector

Attributes

• nh

Functions

• funcSubjuGatorStartGateDetector
• funcget_2d_feature_points
• funcrun

class SubjuGatorStartGateDetector : public StereoBase

Public Functions

SubjuGatorStartGateDetector()

virtual std::vector<cv::Point> get_2d_feature_points(cv::Mat image)

A pure virtual method to segment left/right camera images

See also

get_3d_feature_points()

See also

ROSCameraStream

Parameters

image – left and right ROSimagestream will pass to this method

Returns

a vector of points that define the shape in 2d

void run()

Public Members

ros::NodeHandle nh

SubjuGatorTorpedoBoardDetector

Attributes

• diffusion_time
• feature_block_size
• feature_min_distance
• image_proc_scale
• left_corners
• left_most_recent
• max_features
• right_corners
• right_most_recent

Functions

• funcSubjuGatorTorpedoBoardDetector
• func~SubjuGatorTorpedoBoardDetector

class SubjuGatorTorpedoBoardDetector

Public Functions

SubjuGatorTorpedoBoardDetector()

~SubjuGatorTorpedoBoardDetector()

Public Members

double image_proc_scale

double feature_min_distance

int diffusion_time

int max_features

int feature_block_size

mil_vision::Contour left_corners

mil_vision::Contour right_corners

mil_vision::ImageWithCameraInfo left_most_recent

mil_vision::ImageWithCameraInfo right_most_recent

TorpedoBoardReprojectionCost

Functions

• funcT
• funcTorpedoBoardReprojectionCost
• func~TorpedoBoardReprojectionCost

class TorpedoBoardReprojectionCost

Public Functions

TorpedoBoardReprojectionCost(cv::Matx34d &proj_L, cv::Matx34d &proj_R, std::vector<cv::Point> &corners_L, std::vector<cv::Point> &corners_R)

~TorpedoBoardReprojectionCost()

template<typename T>
bool operator()(const T *const x, const T *const y, const T *const z, const T *const yaw, T *residual) const

Computer Vision

Utility Functions

template<typename PointT>
void sub::voxel_filter(const typename pcl::PointCloud<PointT>::Ptr input_cloud, const typename pcl::PointCloud<PointT>::Ptr output_cloud, float leaf_size)

template<typename PointT>
void sub::statistical_outlier_filter(const typename pcl::PointCloud<PointT>::Ptr input_cloud, const typename pcl::PointCloud<PointT>::Ptr output_cloud, float mean_k, float std_dev_mul_thresh)

template<typename PointT>
size_t sub::closest_point_index_rayOMP(const pcl::PointCloud<PointT> &cloud, const Eigen::Vector3f &direction_pre, const Eigen::Vector3f &line_pt, double &distance_to_ray)

template<typename PointT>
size_t sub::closest_point_index_ray(const pcl::PointCloud<PointT> &cloud, const Eigen::Vector3f &direction_pre, const Eigen::Vector3f &line_pt)

template<typename PointT>
PointT sub::closest_point_ray(const pcl::PointCloud<PointT> &cloud, const Eigen::Vector3f &direction, const Eigen::Vector3f &line_pt, double &distance)

template<typename PointT>
size_t sub::project_uv_to_cloud_index(const pcl::PointCloud<PointT> &cloud, const cv::Point2d &image_point, const image_geometry::PinholeCameraModel camera_model, double &distance)

template<typename PointT>
PointT sub::project_uv_to_cloud(const pcl::PointCloud<PointT> &cloud, const cv::Point2d &image_point, const image_geometry::PinholeCameraModel camera_model, double &distance)

template<typename PointT>
Eigen::Vector3f sub::point_to_eigen(const PointT &pcl_point)

template<typename PointT>
void sub::compute_normals(const typename pcl::PointCloud<PointT>::Ptr input_cloud, PointCloudNT &output_cloud, double normal_radius = 0.05)

template<typename PointT>
void sub::segment_rgb_region_growing(const typename pcl::PointCloud<PointT>::Ptr target_cloud, const std::vector<int> &indices, std::vector<pcl::PointIndices> &clusters, typename pcl::PointCloud<PointT>::Ptr colored_cloud)

template<typename PointT>
void sub::segment_box(const typename pcl::PointCloud<PointT>::Ptr input_cloud, const Eigen::Vector3f &center, const double edge_length, pcl::PointCloud<PointT> &output_cloud)

void anisotropic_diffusion(const cv::Mat &src, cv::Mat &dest, int t_max)

void best_plane_from_combination(const std::vector<Eigen::Vector3d> &point_list, double distance_threshold, std::vector<double> &result_coeffs)

void calc_plane_coeffs(Eigen::Vector3d &pt1, Eigen::Vector3d &pt2, Eigen::Vector3d &pt3, std::vector<double> &plane_coeffs)

double point_to_plane_distance(double a, double b, double c, double d, Eigen::Vector3d pt)

Type Definitions

using ROSCameraStream_Vec3 = mil_vision::ROSCameraStream<cv::Vec3b>

typedef pcl::PointNormal sub::PointNT

typedef pcl::PointCloud<PointNT> sub::PointCloudNT

typedef pcl::PointXYZRGB sub::PointXYZT

typedef pcl::PointCloud<PointXYZT> sub::PointCloudT

typedef pcl::visualization::PointCloudColorHandlerCustom<PointXYZT> sub::ColorHandlerT

SubjuGatorObjectFinder

Attributes

• image_proc_scale
• left_corners
• left_most_recent
• right_corners
• right_most_recent

Functions

• funcSubjuGatorObjectFinder
• func~SubjuGatorObjectFinder

class SubjuGatorObjectFinder

Public Functions

SubjuGatorObjectFinder()

~SubjuGatorObjectFinder()

Public Members

double image_proc_scale

sub::Contour left_corners

sub::Contour right_corners

sub::ImageWithCameraInfo left_most_recent

sub::ImageWithCameraInfo right_most_recent

StereoBase

Functions

• funcStereoBase
• funcget_2d_feature_points
• funcget_3d_feature_points
• funcget_3d_pose
• funcis_stereo_coherent

class StereoBase

Subclassed by SubjuGatorStartGateDetector

Public Functions

StereoBase()

bool is_stereo_coherent()

Check if left+right cameras are publishing and are in sync

See also

sync_thresh_

virtual std::vector<cv::Point> get_2d_feature_points(cv::Mat image) = 0

A pure virtual method to segment left/right camera images

See also

get_3d_feature_points()

See also

ROSCameraStream

Parameters

image – left and right ROSimagestream will pass to this method

Returns

a vector of points that define the shape in 2d

std::unique_ptr<std::vector<Eigen::Vector3d>> get_3d_feature_points(int max_z = 5)

Use stereo intinsics and triangulation to map 2d features to 3d in stereo frame

See also

get_2d_feature_points()

See also

mil_vision::triangulate_Linear_LS

Parameters

max_z – filter points that are greater than the given z value

Returns

vector of points in 3d space or, if failed, a 0-sized vector

std::unique_ptr<Eigen::Affine3d> get_3d_pose(std::vector<Eigen::Vector3d> feature_pts_3d, float z_vector_min = 0.5)

Use 3d points to estimate a normal and a center point and return a pose

See also

best_fit_plane_standard()

Parameters

• feature_pts_3d – a vector of 3d points (currently only supports 4 points)

• z_vector_min – the minimum value the z-component of the normal vector should be

Returns

The translation and rotation with respect to [1,0,0] vector

RvizVisualizer

Attributes

• nh
• rviz_pub

Functions

• funcRvizVisualizer
• funcvisualize_buoy
• funcvisualize_torpedo_board

class RvizVisualizer

Public Functions

RvizVisualizer(std::string rviz_topic)

void visualize_buoy(geometry_msgs::Pose &pose, std::string &frame_id)

void visualize_torpedo_board(geometry_msgs::Pose &pose, Eigen::Quaterniond orientation, std::vector<Eigen::Vector3d> &targets, std::vector<Eigen::Vector3d> &corners3d, std::string &frame_id)

Public Members

ros::Publisher rviz_pub

ros::NodeHandle nh

OccGridUtils

Methods

• defadd_circle
• deffound_marker
• defpublish_grid
• defreset_grid

class subjugator_vision_tools.OccGridUtils(res, width, height, starting_pose, topic_name: str = '/search_grid')

Contains functions for dealing with occupancy grids as well as storing and publishing them.

All distance measured in meters.

add_circle(center, radius)

Adds a circle to the grid. Also used to project the camera’s view onto the grid because it is rotationally intolerant.

found_marker(pose_2d)

Used to mark found markers. It doesn’t matter that this isn’t perfect, we just need to know that something is there.

publish_grid()

Take the occupancy grid and send it out over ros with timestamps and whatnot.

reset_grid(srv)

Resets occupancy grid. I’m using a random service since I don’t really care about getting or returning information here.

Searcher

Methods

• defcheck_searched
• defpolygon_generator
• defreturn_pose

class subjugator_vision_tools.Searcher(searched_area, grid_res, position_offset)

Intended to provide a service that will return a pose to go to in order to search for a missing marker.

Not sure how this will be implemented in its entirety.

check_searched(search_center, search_radius)

Mask out a circle of the searched area around the point, then find the area of the grid in that mask. If the area is above some threshold assume we have searched this area and do not need to search it again.

polygon_generator(n=12)

Using a generator here to allow for easy expansion in the future.

return_pose(srv)

Returns the next pose to go to in order to try and find the marker. Only searches in a circle around our current location (could be extended to searching farther if we need to). This will skip points that have already been searched.

Classification

Functions

• funcClassification
• funcclustering
• funcfiltered
• funczonify

class Classification

Public Functions

Classification(ros::NodeHandle *nh)

pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(pcl::PointCloud<pcl::PointXYZI>::ConstPtr pointCloud)

std::vector<pcl::PointIndices> clustering(pcl::PointCloud<pcl::PointXYZI>::ConstPtr pointCloud)

void zonify(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform, cv::Point &mat_origin)

OGridGen

Functions

• funcOGridGen
• funccallback
• funcclear_ogrid_callback
• funcclear_pcl_callback
• funccluster
• funcdvl_callback
• funcget_objects_callback
• funcpopulate_mat_ogrid
• funcprocess_persistant_ogrid
• funcpublish_big_pointcloud
• funcpublish_ogrid

class OGridGen

Public Functions

OGridGen()

void publish_big_pointcloud(const ros::TimerEvent&)

void callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg)

void dvl_callback(const mil_msgs::RangeStampedConstPtr &dvl)

bool clear_ogrid_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)

bool clear_pcl_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)

bool get_objects_callback(mil_msgs::ObjectDBQuery::Request &req, mil_msgs::ObjectDBQuery::Response &res)

void publish_ogrid()

void process_persistant_ogrid(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_plane)

void populate_mat_ogrid()

mil_msgs::PerceptionObjectArray cluster(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)

Simulation

BagManager

Methods

• asyncdump
• asyncmake_dict
• asyncstart_caching

class subjugator_gazebo_tools.BagManager(nh, diag_dir)

async dump()

Save cached bags and save the next ‘post_cache_time’ seconds.

async make_dict()

Generate caching dictionary from a yaml file.

async start_caching()

Caches bags using big dicts.

{topic_name_1: subscriber_1, topic_name_2: subscriber_2, ...}

Every ‘time_step’ seconds it will save all of the subscribers’ most recent message. This will continue and will fill a cache array that will store ‘pre_cache_time’ seconds worth of messages before erasing the oldest messages.

6DOF Controller

Controller

Methods

• defreset
• defupdate

class rise_6dof.Controller(config: Dict[str, Union[ndarray, bool]])

Input poses must all be in world frame/velocities must all be in body frame (follows convention from Odometry message). The output wrench should be in the body frame.

Parameters

config (Dict[str, Union[np.ndarray, bool]]) – The configuration to use with the controller.

reset() → None

Resets the controller with zero values.

update(dt: float, desired: PoseTwist, current: PoseTwist) → Tuple[Tuple[ndarray, ndarray], Tuple[ndarray, ndarray]]

Updates the controllers with a few values.

Parameters

• dt (float) –

  ???

• desired (PoseTwist) –

  ???

• current (PoseTwist) –

  ???

Dynamics

SubjuGatorDynamics

Attributes

• G
• air_density
• drag_coeffs
• height
• mass
• volume
• water_density

Methods

• clsSubjuGatorDynamics.from_ros_params
• defdrag
• defdynamics
• defgravity_and_buoyancy
• definverse_dynamics
• definverse_dynamics_from_total_wrench
• defnewton_euler_extra_term

class subjugator_system_id.SubjuGatorDynamics(mass: float, rotational_inertia: ndarray, volume: float, drag_coeffs: ndarray, height: float, water_density: float, air_density: float, G: float)

Models the dynamcis of SubjuGator, providing a shared interface for nodes using dynamics or inverse dynamics.

mass

Mass of the sub (in kg).

Type

float

volume

Volume of sub in m^3.

Type

float

drag_coeffs

A 6x1 numpy array. N/(m/s) drag coefficients for X, Y, Z, R, P, Y.

Type

np.ndarray

height

Height of the sub in meters.

Type

float

water_density

Density of water in kg/m^3.

Type

float

air_density

Density of air in kg/m^3.

Type

float

G

Acceleration due to gravity in negative z of world frame in m/s^2.

Type

float

Parameters

• mass (float) – Mass of the sub (in kg).

• rotational_inertia (np.ndarray) – 3x3 rotational inertia matrix.

• volume (float) – Volume of sub in m^3.

• drag_coeffs (np.ndarray) – A 6x1 numpy array. N/(m/s) drag coefficients for X, Y, Z, R, P, Y.

• height (float) – Height of the sub in meters.

• water_density (float) – Density of water in kg/m^3.

• air_density (float) – Density of air in kg/m^3.

• G (float) – Acceleration due to gravity in negative z of world frame in m/s^2.

drag(twist: ndarray) → ndarray

Calculate force/torque due to drag.

Parameters

twist (np.ndarray) – 6x1 linear + angular velocity in body frame in m/s or rad/s.

Returns

6x1 NumPy array of force + torque due to drag in body frame in N or N-M.

Return type

np.ndarray

dynamics(twist: ndarray, accel: ndarray) → ndarray

Calculates the net force + torque which produced a given acceleration.

Parameters

• twist (np.ndarray) – 6x1 linear + angular velocity in body frame in m/s or rad/s.

• accel (np.ndarray) – 6x1 linear + angular acceleration in body frame in m/s^2 or rad/s^2.

Returns

6x1 force + torque in body frame, N or N-M

Return type

np.ndarray

classmethod from_ros_params()

Initialize class from URDF and the /robot_paramters/ ROS param set by upload.launch.

gravity_and_buoyancy(z: float, world_to_body: ndarray) → ndarray

Calculate force/torque due to gravity and buoyancy.

Parameters

• z (float) – Z coordinate of the position of the sub’s center of mass in the world frame.

• world_to_body (np.ndarray) – 3x3 rotation matrix to rotate world frame to body frame.

Returns

Force vector representing buoyancy and gravity.

Return type

np.ndarray

inverse_dynamics(z: float, twist: ndarray, world_to_body: ndarray, wrench: ndarray = array([0., 0., 0., 0., 0., 0.])) → ndarray

Calculates the acceleration of the sub given a wrench, taking into account drag, buoyancy, and gravity.

Parameters

• z (float) – z coordinate of the position of the sub’s center of mass in the world frame.

• twist (np.ndarray) – 6x1 linear + angular velocity in body frame in m/s or rad/s.

• world_to_body (np.ndarray) – 3x3 rotation matrix to rotate world frame to body frame.

• wrench (np.ndarray) – 6x1 force + torque additional applied to body from motors, etc.

Returns

6x1 linear + angular acceleration in body frame in m/s^2

or rad/s^2.

Return type

np.ndarray

inverse_dynamics_from_total_wrench(twist: ndarray, total_wrench: ndarray) → ndarray

Calculates the acceleration of the Sub given the total force/torque on the body.

Parameters

• twist (np.ndarray) – 6x1 linear + angular velocity in body frame in m/s or rad/s.

• wrench (np.ndarray) – 6x1 total force + torque applied to body.

Returns

6x1 linear + angular acceleration in body frame in m/s^2 or rad/s^2.

Return type

np.ndarray

newton_euler_extra_term(twist: ndarray) → ndarray

Calculates the “extra term” in the newton-euler equations for 3D acceleration.

Parameters

twist (np.ndarray) – 6x1 linear + angular velocity in body frame in m/s or rad/s.

Missions

PoseEditor

Methods

• clsPoseEditor.from_Odometry
• clsPoseEditor.from_Odometry_topic
• clsPoseEditor.from_Pose
• clsPoseEditor.from_PoseTwistStamped
• clsPoseEditor.from_PoseTwistStamped_topic
• defabsolute
• defas_Pose
• defas_PoseTwist
• defas_PoseTwistStamped
• defbackward
• defbody_down
• defbody_up
• defdepth
• defdown
• defeast
• defforward
• defheading
• defheading_deg
• defleft
• defnorth
• defrelative
• defrelative_depth
• defroll_left
• defroll_left_deg
• defroll_right
• defroll_right_deg
• defset_orientation
• defset_position
• defsouth
• defturn_left
• defturn_left_deg
• defturn_right
• defturn_right_deg
• defup
• defwest
• defyaw_left
• defyaw_left_deg
• defyaw_right
• defyaw_right_deg
• defzero_roll

class subjugator_missions.PoseEditor(frame_id: str, position: MutableSequence[float], orientation: MutableSequence[float])

Helper class used to create poses for the movement of SubjuGator.

Frequently the methods from this class can be tied together in order to orchestrate multiple movements.

class ExampleMission(SubjuGatorMission):
    pose = self.move() # new pose editor
    down = pose.down(3).left(2) # down 3m, left 2m
    await self.go(down, speed = 0.7) # go to that location at 0.7m/s

str(x)

Pretty prints the position and orientation associated with the pose editor.

repr(x)

Pretty prints the position and orientation associated with the pose editor.

absolute(abs_pos: Sequence[float]) → PoseEditor

Creates a pose editor with an adjusted absolute position. While the orientation of the submarine will be kept, the supplied position is not relative to the sub’s orientation; rather, it is an absolute position, relative only to the world frame.

This method is used by methods such as east() or west() to move the sub in regards to its position only.

as_Pose() → Pose

Constructs a Pose message class from the pose editor.

as_PoseTwist(linear: Sequence[float] = [0, 0, 0], angular: Sequence[float] = [0, 0, 0])

Returns a PoseTwist message class with the pose supplied from the current pose editor. The linear and angular twist are supplied as parameters.

Parameters

• linear (Sequence[float]) – The linear twist.

• angular (Sequence[float]) – The angular twist.

as_PoseTwistStamped(linear: Sequence[float] = [0, 0, 0], angular: Sequence[float] = [0, 0, 0]) → PoseTwistStamped

Returns a PoseTwist message class with the pose supplied from the current pose editor. The linear and angular twist are supplied as parameters. The header includes the frame ID specified in the current pose editor.

Parameters

• linear (Sequence[float]) – The linear twist.

• angular (Sequence[float]) – The angular twist.

backward(distance: float) → PoseEditor

Returns a pose editor that moves the sub backward by some amount.

Parameters

distance (float) – The distance to move backward.

body_down(distance: float)

Creates a pose directly vertically down of the current pose with the same orientation.

Parameters

distance (float) – The number of meters to move down.

body_up(distance: float)

Creates a pose directly vertically up of the current pose with the same orientation.

Parameters

distance (float) – The number of meters to move up.

depth(depth: float) → PoseEditor

Returns an adjusted pose editor with a set depth, relative to z = 0.

Parameters

depth (float) – The depth to head to.

down(distance: float)

Creates a pose directly vertically down of the current pose with the same orientation.

Parameters

distance (float) – The number of meters to move down.

east(distance: float)

Returns a pose editor that has been adjusted by some distance in the absolute east direction.

This method does not take into account the orientation of the sub.

Parameters

distance (float) – The number of meters to move east.

forward(distance: float) → PoseEditor

Returns a pose editor that moves the sub forward by some amount.

Parameters

distance (float) – The distance to move forward.

classmethod from_Odometry(msg: Odometry) → PoseEditor

Creates a pose editor from an Odometry message.

classmethod from_Odometry_topic(topic: str = '/odom') → PoseEditor

Gets a message from a topic sending Odometry messages, and uses the message to construct an editor.

classmethod from_Pose(frame_id: str, msg: Pose) → PoseEditor

Creates a pose editor from a Pose message. Because a pose message does not contain a frame ID, you will need to provide it yourself alongside the message.

classmethod from_PoseTwistStamped(msg: PoseTwistStamped) → PoseEditor

Creates a pose editor from a PoseTwistStamped message.

classmethod from_PoseTwistStamped_topic(topic: str) → PoseEditor

Gets a message from a topic sending PoseTwistStamped messages, and uses the message to construct an editor.

heading(heading: float) → PoseEditor

Returns a pose editor with a heading pointing to a specific direction.

Parameters

heading (float) – The heading, in radians.

heading_deg(heading_deg: float) → PoseEditor

Returns a pose editor with a heading pointing to a specific direction.

Parameters

heading (float) – The heading, in degrees.

left(distance: float) → PoseEditor

Returns a pose editor that moves the sub left by some amount.

Parameters

distance (float) – The distance to move left.

north(distance: float)

Returns a pose editor that has been adjusted by some distance in the absolute north direction.

This method does not take into account the orientation of the sub.

Parameters

distance (float) – The number of meters to move north.

relative(rel_pos: Sequence[float]) → PoseEditor

Returns a new pose editor with a position relevant to the current rotation of the sub.

If you are only interested in setting a new position with the x and y dimensions specified, please see relative_depth() instead.

Parameters

rel_pos (Sequence[float]) – The position relevant to the sub’s current rotation. This should include three numbers, one for each dimension of position.

relative_depth(rel_pos: Sequence[float]) → PoseEditor

Returns a new pose editor using a position relevant to the sub’s current orientation. However, unlike relative(), depth is preserved.

Parameters

rel_pos (Sequence[float]) – The new relative position to use in the editor.

roll_left(angle: float) → PoseEditor

Returns a pose editor rolled to the left by a specific radian amount.

Parameters

angle (float) – The angle to roll, in radians.

roll_left_deg(angle_degrees: float) → PoseEditor

Returns a pose editor rolled to the left by a specific degree amount.

Parameters

angle (float) – The angle to roll, in radians.

roll_right(angle: float) → PoseEditor

Returns a pose editor rolled to the right by a specific radian amount.

Parameters

angle (float) – The angle to roll, in radians.

roll_right_deg(angle_degrees: float) → PoseEditor

Returns a pose editor rolled to the right by a specific degree amount.

Parameters

angle (float) – The angle to roll, in radians.

set_orientation(orientation: Sequence[float]) → PoseEditor

Completely overwrites the orientation of the pose editor with a new orientation

Parameters

orientation (Sequence[float]) – The new orientation to use with the pose editor.

set_position(position: MutableSequence[float]) → PoseEditor

Returns a new pose editor with the same orientation, but a new position.

Parameters

position (Sequence[float]) – A list of three numbers, representing the x, y, and z dimensions of the position.

south(distance: float)

Returns a pose editor that has been adjusted by some distance in the absolute south direction.

This method does not take into account the orientation of the sub.

Parameters

distance (float) – The number of meters to move south.

turn_left(angle: float) → PoseEditor

Returns a pose editor with the orientation yawed to the left by some radian amount.

Parameters

angle (float) – The angle, in radians.

turn_left_deg(angle_degrees: float) → PoseEditor

Returns a pose editor with the orientation yawed to the left by some degree amount.

Parameters

angle (float) – The angle, in degrees.

turn_right(angle: float) → PoseEditor

Returns a pose editor with the orientation yawed to the right by some radian amount.

Parameters

angle (float) – The angle, in radians.

turn_right_deg(angle_degrees: float) → PoseEditor

Returns a pose editor with the orientation yawed to the right by some degree amount.

Parameters

angle (float) – The angle, in degrees.

up(distance: float)

Creates a pose directly vertically up of the current pose with the same orientation.

Parameters

distance (float) – The number of meters to move up.

west(distance: float) → PoseEditor

Returns a pose editor that has been adjusted by some distance in the absolute west direction.

This method does not take into account the orientation of the sub.

Parameters

distance (float) – The number of meters to move west.

yaw_left(angle: float) → PoseEditor

Returns a pose editor with the orientation yawed to the left by some radian amount.

Parameters

angle (float) – The angle, in radians.

yaw_left_deg(angle_degrees: float) → PoseEditor

Returns a pose editor with the orientation yawed to the left by some degree amount.

Parameters

angle (float) – The angle, in degrees.

yaw_right(angle: float) → PoseEditor

Returns a pose editor with the orientation yawed to the right by some radian amount.

Parameters

angle (float) – The angle, in radians.

yaw_right_deg(angle_degrees: float) → PoseEditor

Returns a pose editor with the orientation yawed to the right by some degree amount.

Parameters

angle (float) – The angle, in degrees.

zero_roll() → PoseEditor

Returns a pose editor with zero roll.