123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475 |
- <?xml version="1.0"?>
- <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:property name="M_PI" value="3.1415926535897931" />
- <xacro:macro name="generic_zoom_camera" params="name parent *origin ros_topic cam_info_topic zoom_factor_topic update_rate res_x res_y image_format hfov">
- <joint name="${name}_joint" type="fixed">
- <xacro:insert_block name="origin" />
- <parent link="${parent}"/>
- <child link="${name}_link"/>
- </joint>
-
- <link name="${name}_link">
- <!-- <inertial>
- <mass value="0.001" />
- <origin xyz="0 0 0" rpy="0 0 0" />
- <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <box size="0.01 0.01 0.01" />
- </geometry>
- <material name="Blue">
- <color rgba="0.0 0.0 0.8 1"/>
- </material>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <box size="0.01 0.01 0.01" />
- </geometry>
- </collision>-->
- </link>
-
- <joint name="${name}_optical_joint" type="fixed">
- <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
- <parent link="${name}_link" />
- <child link="${name}_optical_frame"/>
- </joint>
-
- <link name="${name}_optical_frame"/>
-
- <gazebo reference="${name}_link">
- <sensor type="camera" name="${name}_camera_sensor">
- <update_rate>${update_rate}</update_rate>
- <camera>
- <horizontal_fov>${hfov * M_PI/180.0}</horizontal_fov>
- <image>
- <format>${image_format}</format>
- <width>${res_x}</width>
- <height>${res_y}</height>
- </image>
- <clip>
- <near>0.01</near>
- <far>100</far>
- </clip>
- </camera>
- <plugin name="${name}_camera_controller" filename="libgazebo_ros_zoom_camera.so">
- <cameraName>${name}</cameraName>
- <alwaysOn>true</alwaysOn>
- <updateRate>${update_rate}</updateRate>
- <imageTopicName>${ros_topic}</imageTopicName>
- <cameraInfoTopicName>${cam_info_topic}</cameraInfoTopicName>
- <zoomFactorTopicName>${zoom_factor_topic}</zoomFactorTopicName>
- <frameName>${name}_optical_frame</frameName>
- </plugin>
- </sensor>
- </gazebo>
- </xacro:macro>
- </robot>
|