123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188 |
- <?xml version="1.0"?>
- <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:property name="M_PI" value="3.1415926535897931" />
-
- <xacro:property name="default_collision_radius" value="0.025" />
-
- <xacro:property name="visual_radius" value="0.02" />
- <xacro:property name="cable_collision_length" value="0.05" />
- <xacro:macro name="camera360" params="name right_cam_name left_cam_name parent collision_radius:=${default_collision_radius} *origin *calibration dist_between_cameras ros_topic update_rate res_x res_y image_format fov">
-
- <joint name="${name}_center_joint" type="fixed">
- <xacro:insert_block name="origin" />
- <parent link="${parent}"/>
- <child link="${name}_center_link"/>
- </joint>
-
- <link name="${name}_center_link">
- <!-- Sphere body -->
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0"/>
- <geometry>
- <sphere radius="${visual_radius}"/>
- </geometry>
- <material name="Grey">
- <color rgba="0.5 0.5 0.5 1"/>
- </material>
- </visual>
-
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0"/>
- <geometry>
- <sphere radius="${collision_radius}"/>
- </geometry>
- <material name="Grey">
- <color rgba="0.5 0.5 0.5 1"/>
- </material>
- </collision>
-
- <!-- Cable -->
- <!-- <visual>
- <origin xyz="${visual_radius + cable_collision_length/2} 0 0" rpy="0 0 0"/>
- <geometry>
- <box size="${cable_collision_length} 0.03 0.03"/>
- </geometry>
- <material name="Grey">
- <color rgba="0.5 0.5 0.5 1"/>
- </material>
- </visual>-->
-
- <collision>
- <origin xyz="${0.011 + cable_collision_length/2} 0 0" rpy="0 0 0"/>
- <geometry>
- <box size="${cable_collision_length} 0.03 0.03"/>
- </geometry>
- <material name="Grey">
- <color rgba="0.5 0.5 0.5 1"/>
- </material>
- </collision>
-
- </link>
-
- <joint name="${name}_${right_cam_name}_joint" type="fixed">
- <origin xyz="0 ${dist_between_cameras/2} 0" rpy="${M_PI*0.5} 0 ${-M_PI*0.5}"/>
- <parent link="${name}_center_link"/>
- <child link="${name}_${right_cam_name}_link"/>
- </joint>
-
- <link name="${name}_${right_cam_name}_link"/>
-
- <joint name="${name}_${right_cam_name}_optical_joint" type="fixed">
- <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
- <parent link="${name}_${right_cam_name}_link" />
- <child link="${name}_${right_cam_name}_optical_frame"/>
- </joint>
-
- <link name="${name}_${right_cam_name}_optical_frame"/>
-
- <gazebo reference="${name}_${right_cam_name}_link">
- <sensor type="wideanglecamera" name="${name}_${right_cam_name}_camera_sensor">
- <update_rate>${update_rate}</update_rate>
- <camera>
- <!--<horizontal_fov>${hfov * M_PI/180.0}</horizontal_fov>-->
- <horizontal_fov>${fov}</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>
- <lens>
- <!--
- Crashes gazebo on some machines, so revert for now
- <type>custom</type>
- <custom_function>
- <c1>2</c1>
- <c2>2</c2>
- <f>1.0</f>
- <fun>sin</fun>
- </custom_function>
- <cutoff_angle>${fov/2}</cutoff_angle>
- <env_texture_size>1024</env_texture_size>
- -->
- <type>stereographic</type>
- <scale_to_hfov>true</scale_to_hfov>
- <cutoff_angle>1.5707</cutoff_angle>
- <env_texture_size>512</env_texture_size>
- </lens>
- </camera>
- <plugin name="${name}_${right_cam_name}_camera_controller" filename="libgazebo_ros_camera.so">
- <cameraName>${name}/${right_cam_name}</cameraName>
- <imageTopicName>${ros_topic}</imageTopicName>
- <cameraInfoTopicName>not_used</cameraInfoTopicName>
- <frameName>${name}_${right_cam_name}_optical_frame</frameName>
- </plugin>
- </sensor>
- </gazebo>
-
- <joint name="${name}_${left_cam_name}_optical_joint" type="fixed">
- <xacro:insert_block name="calibration" />
- <parent link="${name}_${right_cam_name}_optical_frame"/>
- <child link="${name}_${left_cam_name}_optical_frame"/>
- </joint>
-
- <link name="${name}_${left_cam_name}_optical_frame"/>
- <joint name="${name}_${left_cam_name}_joint" type="fixed">
- <origin xyz="0 0 0" rpy="0 ${-M_PI/2} ${M_PI/2}" />
- <parent link="${name}_${left_cam_name}_optical_frame"/>
- <child link="${name}_${left_cam_name}_link" />
- </joint>
-
- <link name="${name}_${left_cam_name}_link"/>
-
-
- <gazebo reference="${name}_${left_cam_name}_link">
- <sensor type="wideanglecamera" name="${name}_${left_cam_name}_camera_sensor">
- <update_rate>${update_rate}</update_rate>
- <camera>
- <!--<horizontal_fov>${hfov * M_PI/180.0}</horizontal_fov>-->
- <horizontal_fov>${fov}</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>
- <lens>
- <!--
- Crashes gazebo on some machines, so revert for now
- <type>custom</type>
- <custom_function>
- <c1>2</c1>
- <c2>2</c2>
- <f>1.0</f>
- <fun>sin</fun>
- </custom_function>
- <cutoff_angle>${fov/2}</cutoff_angle>
- <env_texture_size>1024</env_texture_size>
- -->
- <type>stereographic</type>
- <scale_to_hfov>true</scale_to_hfov>
- <cutoff_angle>1.5707</cutoff_angle>
- <env_texture_size>512</env_texture_size>
- </lens>
- </camera>
- <plugin name="${name}_${left_cam_name}_camera_controller" filename="libgazebo_ros_camera.so">
- <cameraName>${name}/${left_cam_name}</cameraName>
- <imageTopicName>${ros_topic}</imageTopicName>
- <cameraInfoTopicName>not_used</cameraInfoTopicName>
- <frameName>${name}_${left_cam_name}_optical_frame</frameName>
- </plugin>
- </sensor>
- </gazebo>
- </xacro:macro>
- </robot>
|