Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion config/rm_controllers/sentry.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -182,4 +182,4 @@ controllers:
barrel_controller:
type: effort_controllers/JointPositionController
joint: "barrel_joint"
pid: { p: 30.0, i: 15.0, d: 0.3, i_clamp_max: 0.03, i_clamp_min: -0.03, antiwindup: true, publish_state: false }
pid: { p: 30.0, i: 15.0, d: 0.3, i_clamp_max: 0.03, i_clamp_min: -0.03, antiwindup: true, publish_state: false }
2 changes: 1 addition & 1 deletion config/rm_ecat_hw/device_configurations/dart.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -37,4 +37,4 @@ gpios:
mode: 0
- name: "calibration_right"
pin: 0
mode: 0
mode: 0
2 changes: 1 addition & 1 deletion config/rm_referee/dart.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -7,4 +7,4 @@ rm_referee:
trigger_change:
time_change:
fixed:
flash:
flash:
8 changes: 5 additions & 3 deletions config/vision/camera.yaml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
camera:
hk_camera:
image_width: 1440
image_height: 1080
pixel_format: bgr8
Expand All @@ -8,8 +8,10 @@ camera:
camera_sn: 00J90631052
frame_rate: 1000
exposure_auto: false
exposure_value: 1500
exposure_value: 1000
exposure_value_windmill: 3000
gamma_selector: 2
gamma_value: 0.5
enable_imu_trigger: true
enable_imu_trigger: false
is_fps_down: false
target_fps: 40.0
17 changes: 17 additions & 0 deletions config/vision/camera_left.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
hk_camera_left:
# 1280×1024 640×512
image_width: 1280
image_height: 1024
pixel_format: bgr8
camera_info_url: 'file:///home/hiling/.ros/camera_info/DA8835803_left.yaml'
camera_name: hk_camera_left
camera_frame_id: camera_optical_frame_left
frame_id: camera_optical_frame_left
camera_sn: DA8835805
frame_rate: 15
exposure_auto: false
exposure_value: 1000
gamma_selector: 2
gamma_value: 0.5
enable_imu_trigger: true
sleep_time: 2
17 changes: 17 additions & 0 deletions config/vision/camera_right.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
hk_camera_right:
# 1280×1024 640×512
image_width: 1280
image_height: 1024
pixel_format: bgr8
camera_info_url: 'file:///home/hiling/.ros/camera_info/test_cam_right.yaml'
camera_name: hk_camera_right
camera_frame_id: camera_optical_frame_right
frame_id: camera_optical_frame_right
camera_sn: DA8835803
frame_rate: 15
exposure_auto: false
exposure_value: 1000
gamma_selector: 2
gamma_value: 0.5
enable_imu_trigger: true
sleep_time: 1
22 changes: 0 additions & 22 deletions config/vision/sentry.yaml

This file was deleted.

33 changes: 15 additions & 18 deletions launch/vision/camera.launch
Original file line number Diff line number Diff line change
@@ -1,25 +1,22 @@
<launch>
<arg name="imu_trigger" default="false"/>
<arg name="camera_type" default="$(env CAMERA_TYPE)" doc="Camera type"/>
<arg name="camera_class" default="$(env CAMERA_CLASS)"/>
<arg name="camera_name" default="camera"/>
<arg name="camera_frame" default="camera_optical_frame"/>
<arg name="hw_name" default="$(env HW_NAME)" doc="[rm_ecat_hw, rm_hw]"/>
<arg name="manager_name" default="vision_nodelet"/>
<arg name="camera_class" default="HKCameraNodelet"/>
<arg name="camera_type" default="hk_camera"/>
<arg name="suffix" default=""/>
<arg name="hw_name" default="rm_ecat_hw" doc="[rm_ecat_hw, rm_hw]"/>
<arg name="image_topic"
default="$(eval '/' + arg('camera_type') + ('_' + arg('suffix') if arg('suffix') else '') + '/image_raw')"/>
<arg name="camera_info_topic"
default="$(eval '/' + arg('camera_type') + ('_' + arg('suffix') if arg('suffix') else '') + '/camera_info')"/>

<rosparam file="$(find rm_config)/config/vision/camera.yaml" command="load"/>

<node pkg="nodelet" type="nodelet" name="vision_nodelet" args="manager"
output="screen" respawn="true">
<param name="num_worker_threads" value="8"/>
</node>

<node pkg="nodelet" type="nodelet" name="$(arg camera_name)"
args="load $(arg camera_type)/$(arg camera_class) vision_nodelet"
<rosparam file="$(eval find('rm_detection_tradition') + '/config/camera' + ('_' + arg('suffix') if arg('suffix') else '') + '.yaml')" command="load"/>
<node pkg="nodelet" type="nodelet" name="$(eval arg('camera_type') + ('_' + arg('suffix') if arg('suffix') else ''))"
args="load $(arg camera_type)/$(arg camera_class) $(arg manager_name)"
output="screen" respawn="true">
<param name="camera_frame_id" value="$(arg camera_frame)"/>
<param name="enable_imu_trigger" value="$(arg imu_trigger)"/>
<remap from="$(arg camera_name)/image_raw" to="/$(arg camera_type)/$(arg camera_name)/image_raw"/>
<remap from="/$(arg camera_name)/imu_trigger" to="/$(arg hw_name)/imu_trigger"/>
<remap from="$(arg camera_type)/image_raw" to="$(arg image_topic)"/>
<remap from="$(arg camera_type)/camera_info" to="$(arg camera_info_topic)"/>
<remap from="/rm_hw/gimbal_imu/trigger_time" to="/$(arg hw_name)/gimbal_imu/trigger_time"/>
</node>
</launch>
</launch>
53 changes: 0 additions & 53 deletions launch/vision/sentry_camera.launch

This file was deleted.

42 changes: 0 additions & 42 deletions launch/vision/sentry_vision_loader.launch

This file was deleted.

45 changes: 0 additions & 45 deletions launch/vision/vision_loader.launch

This file was deleted.

99 changes: 99 additions & 0 deletions launch/vision/vision_main_node.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
<launch>
<arg name="camera_type" default="hk_camera"/>
<arg name="manager_name" default="vision_nodelet"/>

<!-- nodelet manager name -->
<arg name="use_back" default="false"/>

<arg name="use_left" default="false"/>
<arg name="use_right" default="false"/>

<!-- ========== main thread ========== -->
<!-- main nodelet manager -->
<node pkg="nodelet" type="nodelet" name="$(arg manager_name)" args="manager" output="screen"/>

<!-- Camera -->
<include file="$(find rm_target_tracker)/launch/camera.launch">
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="manager_name" value="$(arg manager_name)"/>
<arg name="suffix" value=""/>
</include>

<!-- Detection -->
<include file="$(find rm_detection_tradition)/launch/detection_tradition.launch">
<arg name="manager_name" value="$(arg manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value=""/>
</include>

<!-- Pose solver -->
<include file="$(find rm_pose_solver)/launch/pose_solver.launch">
<arg name="manager_name" value="$(arg manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value=""/>
</include>

<!-- Target Tracker -->
<include file="$(find rm_target_tracker)/launch/target_tracker.launch">
<arg name="manager_name" value="$(arg manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value=""/>
</include>

<!-- ========== back thread ========== -->
<group if="$(arg use_back)">
<arg name="sub_manager_name" default="vision_nodelet_back"/>

<node pkg="nodelet" type="nodelet" name="$(arg sub_manager_name)" args="manager" output="screen"/>

<include file="$(find rm_target_tracker)/launch/camera.launch">
<arg name="manager_name" value="$(arg sub_manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value="back"/>
</include>

<include file="$(find rm_detection_tradition)/launch/detection_tradition.launch">
<arg name="manager_name" value="$(arg sub_manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value="back"/>
</include>
</group>

<!-- ========== left thread ========== -->
<group if="$(arg use_left)">
<arg name="sub_manager_name" default="vision_nodelet_left"/>

<node pkg="nodelet" type="nodelet" name="$(arg sub_manager_name)" args="manager" output="screen"/>

<include file="$(find rm_target_tracker)/launch/camera.launch">
<arg name="manager_name" value="$(arg sub_manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value="left"/>
</include>

<include file="$(find rm_detection_tradition)/launch/detection_tradition.launch">
<arg name="manager_name" value="$(arg sub_manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value="left"/>
</include>
</group>

<!-- ========== right thread ========== -->
<group if="$(arg use_right)">
<arg name="sub_manager_name" default="vision_nodelet_right"/>

<node pkg="nodelet" type="nodelet" name="$(arg sub_manager_name)" args="manager" output="screen"/>

<include file="$(find rm_target_tracker)/launch/camera.launch">
<arg name="manager_name" value="$(arg sub_manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value="right"/>
</include>

<include file="$(find rm_detection_tradition)/launch/detection_tradition.launch">
<arg name="manager_name" value="$(arg sub_manager_name)"/>
<arg name="camera_type" value="$(arg camera_type)"/>
<arg name="suffix" value="right"/>
</include>
</group>
</launch>