Skip to content

Commit

Permalink
work
Browse files Browse the repository at this point in the history
  • Loading branch information
@cbrxyz
cbrxyz committed Oct 23, 2024
1 parent f4b15d6 commit 235e343
Show file tree
Hide file tree
Showing 6 changed files with 68 additions and 205 deletions.
2 changes: 1 addition & 1 deletion NaviGator/gnc/navigator_path_planner/navigator_path_planner/params.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@

# SPEED AND THRUST LIMITS

velmax_pos = np.array([2.4, 1.2, 0.22]) # (m/s, m/s, rad/s), body-frame forward
velmax_pos = np.array([1.0, 1.2, 0.18]) # (m/s, m/s, rad/s), body-frame forward
velmax_neg = np.array([-1.2, -1.2, -0.22]) # (m/s, m/s, rad/s), body-frame backward
thrust_max = np.array([220, 220, 220, 220]) # N, per thruster

Expand Down
141 changes: 0 additions & 141 deletions NaviGator/gnc/navigator_thrust_mapper/'
View file

This file was deleted.

7 changes: 5 additions & 2 deletions NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,8 +108,11 @@ def publish_thrusts(self) -> None:
if len(thrusts) != len(self.publishers):
rospy.logfatal("Number of thrusts does not equal number of publishers")
return
for i in range(len(self.publishers)):
commands[i].setpoint = thrusts[i]
for i, pub in enumerate(self.publishers):
if pub.name == "/BL_motor/cmd":
commands[i].setpoint = thrusts[i] * 0.85
else:
commands[i].setpoint = thrusts[i]
if not self.is_vrx and not self.is_sim:
for i in range(len(self.publishers)):
self.joint_state_msg.effort[i] = commands[i].setpoint
Expand Down
117 changes: 58 additions & 59 deletions NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch
View file
Original file line number Diff line number Diff line change
@@ -1,68 +1,67 @@
<?xml version="1.0"?>
<launch>

<arg name="weights_model1" default="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
<arg name="weights_model2" default="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
<arg name="weights_model3" default="$(find yolov7_ros)/src/mil_weights/stc.pt" />
<arg name="data_yaml_model1" default="$(find yolov7_ros)/src/data/vrx_buoys.yaml" />
<arg name="data_yaml_model2" default="$(find yolov7_ros)/src/data/vrx_buoys.yaml" />
<arg name="data_yaml_model3" default="$(find yolov7_ros)/src/data/stc.yaml" />
<arg name="main_image_topic" default="/camera/front/left/image_color"/>
<arg name="image_topic_model1" default="/yolov7/model1" />
<arg name="image_topic_model2" default="/yolov7/model2" />
<arg name="out_topic_model1" default="detections_model1" />
<arg name="out_topic_model2" default="detections_model2" />
<arg name="out_topic_model3" default="stc_detections_model" />
<arg name="conf_thresh" default="0.25" />
<arg name="image_size" default="640" />
<arg name="device" default="cpu" />
<arg name="use_yolo_model1" default="False"/>
<arg name="use_yolo_model2" default="False"/>
<arg name="use_yolo_model3" default="True"/>
<arg name="weights_model1" default="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
<arg name="weights_model2" default="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
<arg name="weights_model3" default="$(find yolov7_ros)/src/mil_weights/stc.pt" />
<arg name="data_yaml_model1" default="$(find yolov7_ros)/src/data/vrx_buoys.yaml" />
<arg name="data_yaml_model2" default="$(find yolov7_ros)/src/data/vrx_buoys.yaml" />
<arg name="data_yaml_model3" default="$(find yolov7_ros)/src/data/stc.yaml" />
<arg name="main_image_topic" default="/camera/front/left/image_color" />
<arg name="image_topic_model1" default="/yolov7/model1" />
<arg name="image_topic_model2" default="/yolov7/model2" />
<arg name="out_topic_model1" default="detections_model1" />
<arg name="out_topic_model2" default="detections_model2" />
<arg name="out_topic_model3" default="stc_detections_model" />
<arg name="conf_thresh" default="0.7" />
<arg name="image_size" default="640" />
<arg name="device" default="cpu" />
<arg name="use_yolo_model1" default="False" />
<arg name="use_yolo_model2" default="False" />
<arg name="use_yolo_model3" default="True" />

<node name="testing_arbiter" pkg="topic_tools" type="demux"
args="$(arg main_image_topic) $(arg image_topic_model1) $(arg image_topic_model2) /yolov7/none">
<remap from="demux" to="/yolov7"/>
</node>
<node name="testing_arbiter" pkg="topic_tools" type="demux" args="$(arg main_image_topic) $(arg image_topic_model1) $(arg image_topic_model2) /yolov7/none">
<remap from="demux" to="/yolov7" />
</node>

<!-- Use YOLOv7 -->
<include if="$(arg use_yolo_model1)" file="$(find yolov7_ros)/launch/yolov7.launch">
<arg name="weights" value="$(arg weights_model1)"/>
<arg name="image_topic" value="$(arg image_topic_model1)"/>
<arg name="out_topic" value="$(arg out_topic_model1)"/>
<arg name="conf_thresh" value="$(arg conf_thresh)"/>
<arg name="image_size" value="$(arg image_size)"/>
<arg name="device" value="$(arg device)"/>
<arg name="data_yaml" value="$(arg data_yaml_model1)"/>
</include>
<!-- Use YOLOv7 -->
<include if="$(arg use_yolo_model1)" file="$(find yolov7_ros)/launch/yolov7.launch">
<arg name="weights" value="$(arg weights_model1)" />
<arg name="image_topic" value="$(arg image_topic_model1)" />
<arg name="out_topic" value="$(arg out_topic_model1)" />
<arg name="conf_thresh" value="$(arg conf_thresh)" />
<arg name="image_size" value="$(arg image_size)" />
<arg name="device" value="$(arg device)" />
<arg name="data_yaml" value="$(arg data_yaml_model1)" />
</include>

<!-- Use YOLOv7 -->
<include if="$(arg use_yolo_model2)" file="$(find yolov7_ros)/launch/yolov7.launch">
<arg name="weights" value="$(arg weights_model2)"/>
<arg name="image_topic" value="$(arg image_topic_model2)"/>
<arg name="out_topic" value="$(arg out_topic_model2)"/>
<arg name="conf_thresh" value="$(arg conf_thresh)"/>
<arg name="image_size" value="$(arg image_size)"/>
<arg name="device" value="$(arg device)"/>
<arg name="data_yaml" value="$(arg data_yaml_model2)"/>
</include>
<!-- Use YOLOv7 -->
<include if="$(arg use_yolo_model2)" file="$(find yolov7_ros)/launch/yolov7.launch">
<arg name="weights" value="$(arg weights_model2)" />
<arg name="image_topic" value="$(arg image_topic_model2)" />
<arg name="out_topic" value="$(arg out_topic_model2)" />
<arg name="conf_thresh" value="$(arg conf_thresh)" />
<arg name="image_size" value="$(arg image_size)" />
<arg name="device" value="$(arg device)" />
<arg name="data_yaml" value="$(arg data_yaml_model2)" />
</include>

<!-- Use YOLOv7 -->
<include if="$(arg use_yolo_model3)" file="$(find yolov7_ros)/launch/yolov7.launch">
<arg name="weights" value="$(arg weights_model3)"/>
<arg name="image_topic" value="/stc_mask_debug"/>
<arg name="out_topic" value="$(arg out_topic_model3)"/>
<arg name="conf_thresh" value="$(arg conf_thresh)"/>
<arg name="image_size" value="$(arg image_size)"/>
<arg name="device" value="$(arg device)"/>
<arg name="data_yaml" value="$(arg data_yaml_model3)"/>
</include>
<!-- Use YOLOv7 -->
<include if="$(arg use_yolo_model3)" file="$(find yolov7_ros)/launch/yolov7.launch">
<arg name="weights" value="$(arg weights_model3)" />
<arg name="image_topic" value="/stc_mask_debug" />
<arg name="out_topic" value="$(arg out_topic_model3)" />
<arg name="conf_thresh" value="$(arg conf_thresh)" />
<arg name="image_size" value="$(arg image_size)" />
<arg name="device" value="$(arg device)" />
<arg name="data_yaml" value="$(arg data_yaml_model3)" />
</include>

<arg name="train" default="False" />
<node pkg="navigator_vision" type="classifier.py" name="classifier" output="screen">
<param name="debug" value="True" />
<param name="image_topic" value="$(arg main_image_topic)" />
<param name="model_location" value="config/model" />
<param name="train" value="$(arg train)" />
</node>
<arg name="train" default="False" />
<node pkg="navigator_vision" type="classifier.py" name="classifier" output="screen">
<param name="debug" value="True" />
<param name="image_topic" value="$(arg main_image_topic)" />
<param name="model_location" value="config/model" />
<param name="train" value="$(arg train)" />
</node>
</launch>
2 changes: 1 addition & 1 deletion NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ async def run(self, args):
# Parameters:
scan_code = False
return_to_start = True
circle_radius = 5
circle_radius = 10
circle_direction = "cw"
yaw_offset = 1.57
self.traversal_distance = 3
Expand Down
4 changes: 3 additions & 1 deletion mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -223,14 +223,16 @@ void Node::velodyne_cb(const sensor_msgs::PointCloud2ConstPtr& pcloud)
pub_pcl_.publish(filtered_accrued);

// Skip object detection if all points where filtered out
// Get object clusters from persistent pointcloud
if ((*filtered_accrued).empty())
ROS_WARN_ONCE("Filtered pointcloud had no points. Consider changing filter parameters.");

// Get object clusters from persistent pointcloud
clusters_t clusters = detector_.get_clusters(filtered_accrued);
std::cout << "clusters were gotten" << std::endl;

// Associate current clusters with old ones
ass.associate(*objects_, *filtered_accrued, clusters, thrust_back);
std::cout << "associator associated" << std::endl;
}

bool Node::bounds_update_cb(const mil_bounds::BoundsConfig& config)
Expand Down

0 comments on commit 235e343

Please sign in to comment.