Normalize axis, camera topics, zoom for link750

Author: RomanRobotnik

urdf/link_750_nh.urdf.xacro

@@ -7,10 +7,10 @@
   <xacro:property name="ptz_mechanical_reduction" value="1.0"/>  
   <xacro:property name="PI" value="3.14159265359"/>
 
-  <xacro:macro name="sensor_link_750_nh" params="prefix parent *origin far:=^|8.0 near:=^|0.05 prefix_topic:=^ptz_camera include_inertial:=^|true">
+  <xacro:macro name="sensor_link_750_nh" params="prefix parent *origin far:=^|8.0 near:=^|0.05 prefix_topic:=^ptz_camera include_inertial:=^|true fps:=^|30.0">
 
     <joint name="${prefix}_joint" type="fixed">
-      <axis xyz="0 1 0"/>
+      <axis xyz="0 0 1"/>
       <xacro:insert_block name="origin"/>
       <parent link="${parent}"/>
       <child link="${prefix}_base_link"/>
@@ -41,7 +41,7 @@
     </link>
 
     <joint name="${prefix}_pan_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
+      <axis xyz="0 0 1"/>
       <origin xyz="0.0 0.0 0.0318" rpy="0 0 0"/>
       <!-- check the displacement -->
       <parent link="${prefix}_base_link"/>
@@ -85,8 +85,8 @@
     </transmission>
 
     <joint name="${prefix}_tilt_joint" type="revolute">
-      <axis xyz="0 -1 0"/>
-      <origin xyz="0.0 0.0 0.102" rpy="0 0 0"/>
+      <axis xyz="0 0 1"/>
+      <origin xyz="0.0 0.0 0.102" rpy="${PI/2} 0 0"/>
       <parent link="${prefix}_pan_link"/>
       <child link="${prefix}_tilt_link"/>
       <limit effort="${ptz_joint_effort_limit}" velocity="${ptz_joint_velocity_limit}" lower="-1.5708" upper="1.5708"/>
@@ -101,7 +101,7 @@
         </inertial>
       </xacro:if>
       <visual>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
         <material name="grey_color">
           <color rgba="0.1 0.1 0.1 1"/>
         </material>
@@ -110,7 +110,7 @@
         </geometry>
       </visual>
       <collision>
-        <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+        <origin xyz="0.0 0.0 0" rpy="${PI/2} 0 0"/>
         <geometry>
           <mesh filename="package://robotnik_sensors/meshes/link_750_tilt.stl" scale="1.0 1.0 1.0"/>
         </geometry>
@@ -127,8 +127,35 @@
       </actuator>
     </transmission>
 
+    <joint name="${prefix}_zoom_joint" type="prismatic">
+      <axis xyz="0 0 1"/>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <parent link="${prefix}_optical_frame_link"/>
+      <child link="${prefix}_zoom_link"/>
+      <limit effort="1000.0" velocity="1" lower="0" upper="10000"/>
+    </joint>
+    <link name="${prefix}_zoom_link">
+      <xacro:if value="${include_inertial}">
+        <inertial>
+          <mass value="0.1"/>
+          <origin xyz="0 0 0" rpy="0 0 0"/>
+          <xacro:solid_cuboid_inertia m="0.1" w="0.11" h="0.11" d="0.1" />
+        </inertial>
+      </xacro:if>
+    </link>
+    <transmission name="${prefix}_zoom_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}_zoom_joint">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}zoom_motor">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
     <joint name="${prefix}_frame_joint" type="fixed">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <origin xyz="0.06 0 0" rpy="${-PI/2} 0 0"/>
       <parent link="${prefix}_tilt_link"/>
       <child link="${prefix}_frame_link"/>
     </joint>
@@ -141,9 +168,9 @@
         </inertial>
       </xacro:if>
     </link>
-    <!-- Optical frame -->
+    <!-- Optical frame ${-M_PI/2} -->
     <joint name="${prefix}_optical_joint" type="fixed">
-      <origin xyz="0.02 0 0" rpy="0 0 0"/>
+      <origin xyz="0.0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}"/>
       <parent link="${prefix}_frame_link"/>
       <child link="${prefix}_optical_frame_link"/>
     </joint>
@@ -168,9 +195,9 @@
 
   <xacro:macro name="sensor_link_750_nh_gazebo">
 
-    <gazebo reference="${prefix}_optical_frame_link"> <!-- the image will be generated from this (${name}_frame) point of view, with the front being the X axis -->
+    <gazebo reference="${prefix}_frame_link"> <!-- the image will be generated from this (${name}_frame) point of view, with the front being the X axis -->
       <sensor type="camera" name="${prefix}_sensor">
-        <update_rate>30.0</update_rate>
+        <update_rate>${fps}</update_rate>
         <camera name="${prefix_topic}">
           <horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
           <image>
@@ -186,7 +213,7 @@
         <plugin name="${prefix}_controller" filename="libgazebo_ros_camera.so">
           <alwaysOn>true</alwaysOn>
           <updateRate>0.0</updateRate>
-          <cameraName>top_rgb_${prefix_topic}</cameraName>
+          <cameraName>${prefix_topic}/rgb</cameraName>
           <imageTopicName>image_raw</imageTopicName>
           <cameraInfoTopicName>camera_info</cameraInfoTopicName>
           <frameName>${prefix}_optical_frame_link</frameName>
@@ -202,7 +229,7 @@
         </plugin>
       </sensor>
       <sensor type="camera" name="thermal_${prefix}_sensor">
-        <update_rate>30.0</update_rate>
+        <update_rate>${fps}</update_rate>
         <camera>
           <horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
           <image>
@@ -218,11 +245,11 @@
 
         <plugin name="thermal_camera_controller" filename="libgazebo_ros_thermal_camera.so">
           <alwaysOn>true</alwaysOn>
-          <updateRate>10</updateRate>
-          <cameraName>top_thermal_${prefix_topic}</cameraName>
+          <updateRate>${fps}</updateRate>
+          <cameraName>${prefix_topic}/thermal</cameraName>
           <imageTopicName>image_raw</imageTopicName>
           <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-          <frameName>${prefix}_optical_frame_link</frameName>
+          <frameName>${prefix}_frame_link</frameName>
         </plugin>
       </sensor>
     </gazebo>