add rfans16

open-rdc · Dec 23, 2023 · 512d69b · 512d69b
1 parent 5d389ce
commit 512d69b
Show file tree

Hide file tree

Showing 5 changed files with 62 additions and 7 deletions.
diff --git a/orne_box_bringup/launch/includes/rfans16.launch b/orne_box_bringup/launch/includes/rfans16.launch
@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="namespace" default="rfans"/>
+  <arg name="output" default="screen"/>
+  <arg name="respawn" default="true"/>
+  <arg name="revise_angle_128" value="0.027721,0.002361,-0.043092,-0.023711,0.003442,-0.046271,-0.018255,0.036872,-0.048702,-0.025516,0.002106,-0.040973,-0.019308,0.001663,-0.045102,-0.015489,0.001148,-0.047884,-0.018850,0.016742,-0.044528,-0.033280,0.001597,-0.039345,1,1.8,1,1.8,0.15,0.15,"/>
+  <arg name="revise_angle_32" value="0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0,0, 0, 0,0, 0, 0,0, 0, 0,0, 0, 0,45,0,0,0,0.1,0,"/>
+  <arg name="read_fast" default="false"/>
+  <arg name="read_once" default="false"/>
+  <arg name="repeat_delay" default="0.0"/>
+  <arg name="frame_id" default="surestar"/>
+  <group ns="$(arg namespace)" clear_params="true">
+    <param name="model" value="R-Fans-16"/>
+    <node pkg="rfans_driver" type="driver_node" name="rfans_driver" output="screen">
+      <param name="advertise_name" value="surestar_packets"/>
+      <param name="control_name" value="surestar_control"/>
+      <param name="device_ip" value="192.168.0.3"/>
+      <param name="device_port" value="2014"/>
+      <param name="rps" value="10"/>
+      <param name="pcap" value=""/>
+      <param name="data_level" value="3"/>
+      <param name="use_double_echo" value="false"/>
+      <param name="read_fast" value="$(arg read_fast)"/>
+      <param name="read_once" value="$(arg read_once)"/>
+      <param name="repeat_delay" value="$(arg repeat_delay)"/>
+    </node>
+    <node pkg="rfans_driver" type="calculation_node" name="calculation_node">
+      <param name="advertise_name" value="surestar_points"/>
+      <param name="subscribe_name" value="surestar_packets"/>
+      <param name="frame_id" value="$(arg frame_id)"/>
+      <param name="use_gps" value="false"/>
+      <param name="revise_angle_128" value="$(arg revise_angle_128)"/>
+      <param name="revise_angle_32" value="$(arg revise_angle_32)"/>
+    </node>
+    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
+       <param name="max_height" value="0.2"/>	    
+       <param name="min_height" value="-0.2"/>
+       <param name="range_max" value="100.0"/>
+       <param name="range_min" value="0.0"/>
+       <param name="angle_increment" value="0.0035"/>
+       <remap from="cloud_in" to="rfans_driver/surestar_points"/>
+       <remap from="/rfans/scan" to="/scan"/>
+    </node>
+  </group>
+</launch>
diff --git a/orne_box_bringup/launch/orne_box3.launch b/orne_box_bringup/launch/orne_box3.launch
@@ -3,13 +3,17 @@
 <launch>
 
   <include file="$(find icart_mini_driver)/launch/icart_mini_drive.launch">
-    <arg name="model"           value="$(find xacro)/xacro '$(find orne_box_description)/urdf/orne_box.urdf.xacro'" />
+    <arg name="model"           value="$(find xacro)/xacro '$(find orne_box_description)/urdf/orne_box3.urdf.xacro'" />
     <arg name="ypspur_params"   value="$(find orne_box_setup)/config/orne_box.param" />
     <arg name="scan_dev"        value="/dev/sensors/hokuyo_H1885238" />
   </include>
 
   <include file="$(find orne_box_bringup)/launch/includes/adis16465.launch"/>
   <include file="$(find orne_box_bringup)/launch/includes/base.launch.xml"/>
-
+<!---
+  <include file="$(find orne_box_bringup)/launch/includes/rfans16.launch">
+    <arg name="frame_id"        value="rfans16_link" />
+  </include>
+-->
 </launch>
 
diff --git a/orne_box_bringup/package.xml b/orne_box_bringup/package.xml
@@ -11,4 +11,5 @@
   <build_depend>icart_mini_gazebo</build_depend>
   <exec_depend>icart_mini_driver</exec_depend>
   <exec_depend>icart_mini_gazebo</exec_depend>
+  <exec_depend>pointcloud_to_laserscan</exec_depend>
 </package>
diff --git a/orne_box_description/urdf/orne_box3.urdf.xacro b/orne_box_description/urdf/orne_box3.urdf.xacro
@@ -188,9 +188,14 @@
   </xacro:sensor_imu>
 
 
+  <xacro:include filename="$(find icart_mini_description)/urdf/sensors/hokuyo.urdf.xacro"/>
+  <xacro:sensor_hokuyo name="hokuyo" parent="base_link">
+    <origin xyz="0.374295 0 0.2185" rpy="${-PI} 0 0"/>
+  </xacro:sensor_hokuyo>
+
   <xacro:include filename="$(find orne_box_description)/urdf/sensor/rfans16.urdf.xacro"/>
   <xacro:sensor_rfans16 name="rfans16" parent="base_link">
-    <origin xyz="0.23 0 0.40" rpy="0 0 0"/>
+    <origin xyz="0.23 0 0.40" rpy="0 0 ${PI}"/>
   </xacro:sensor_rfans16>
 
   <transmission name="tran1">

diff --git a/orne_box_setup/config/orne_box.param b/orne_box_setup/config/orne_box.param
@@ -27,10 +27,10 @@ MOTOR_VC 630.0
 MOTOR_TC 0.01515
 MOTOR_M_INERTIA 0
 TIRE_M_INERTIA 0.02
-MASS 20
-MOMENT_INERTIA 0.1
-GAIN_KP 120
-GAIN_KI 300
+MASS 30
+MOMENT_INERTIA 1.8
+GAIN_KP 80
+GAIN_KI 110
 TORQUE_VISCOS 0.00001
 TORQUE_NEWTON 0.00200