这周的任务就是修改代码！
<?xml version="1.0"?>
<!-- Copyright (c) 2016 The UUV Simulator Authors.
All rights reserved.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Macro for a minimal collision block (for when you do not want collision block but gazebo needs one) -->
<xacro:macro name="no_collision">
<collision>
<geometry>
<cylinder length="${0.000001}" radius="${0.000001}" />
</geometry>
<origin xyz="0 0 0" rpy="0 ${0.5*pi} 0"/>
</collision>
</xacro:macro>
<!-- Side scan sonar -->
<xacro:macro name="sidescan_sonar" params="namespace parent_link suffix *origin">
<xacro:multibeam_sonar
namespace="${namespace}"
suffix="${suffix}"
parent_link="${parent_link}"
topic="sss_${suffix}"
mass="0.015"
update_rate="2"
samples="60"
fov="${pi/3.0}"
range_min="1.0"
range_max="200.0"
range_stddev="0.027"
mesh="">
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
<xacro:insert_block name="origin" />
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</xacro:multibeam_sonar>
</xacro:macro>
<!-- Inspired by m450-130: http://www.blueview.com/products/2d-imaging-sonar/m900-series/ -->
<xacro:macro name="forward_multibeam_sonar_m450_130" params="namespace parent_link *origin">
<xacro:multibeam_sonar
namespace="${namespace}"
suffix=""
parent_link="${parent_link}"
topic="sonar"
mass="0.015"
update_rate="25"
samples="768"
fov="2.27"
range_min="1.0"
range_max="100.0"
range_stddev="0.027"
mesh="">
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
<xacro:insert_block name="origin" />
<visual>
<geometry>
<mesh filename="file://$(find uuv_sensor_ros_plugins)/meshes/m450-130.dae" scale="1 1 1"/>
</geometry>
</visual>
</xacro:multibeam_sonar>
</xacro:macro>
<xacro:macro name="forward_multibeam_p900" params="namespace parent_link *origin">
<xacro:multibeam_sonar
namespace="${namespace}"
suffix=""
parent_link="${parent_link}"
topic="sonar"
mass="0.02"
update_rate="15"
samples="512"
fov="1.5708"
range_min="1.0"
range_max="100.0"
range_stddev="0.027"
mesh="">
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
<xacro:insert_block name="origin" />
<visual>
<geometry>
<!-- <mesh filename="file://$(find uuv_sensor_ros_plugins)/meshes/p900.dae" scale="1 1 1"/> -->
<mesh filename="model://blueview_p900_nps_multibeam/meshes/p900.dae" scale="1 1 1"/>
</geometry>
</visual>
</xacro:multibeam_sonar>
</xacro:macro>
<xacro:macro name="multibeam_sonar"
params="namespace suffix parent_link topic mass update_rate
samples fov range_min range_max range_stddev mesh
*inertia *origin *visual">
<!-- Sensor link -->
<link name="${namespace}/sonar${suffix}_link">
<inertial>
<xacro:insert_block name="inertia" />
<mass value="${mass}" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
<xacro:insert_block name="visual" />
<xacro:no_collision/>
</link>
<joint name="${namespace}_sonar${suffix}_joint" type="revolute">
<xacro:insert_block name="origin" />
<parent link="${parent_link}" />
<child link="${namespace}/sonar${suffix}_link" />
<limit upper="0" lower="0" effort="0" velocity="0" />
<axis xyz="1 0 0"/>
</joint>
<gazebo reference="${namespace}/sonar${suffix}_link">
<sensor type="gpu_ray" name="sonar${suffix}">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>${update_rate}</update_rate>
<ray>
<scan>
<horizontal>
<samples>${samples}</samples>
<resolution>1</resolution>
<min_angle>-${0.5*fov}</min_angle>
<max_angle>${0.5*fov}</max_angle>
</horizontal>
</scan>
<range>
<min>${range_min}</min>
<max>${range_max}</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>${range_stddev}</stddev>
</noise>
</ray>
<plugin name="sonar${suffix}_controller" filename="libgazebo_ros_gpu_laser.so">
<topicName>${topic}</topicName>
<frameName>sonar${suffix}_link</frameName>
</plugin>
<!-- TODO: Separate switchable sonar version in another macro -->
<!-- <plugin name="switchable_sonar{suffix}_ros_interface" filename="libuuv_gazebo_ros_switchable_gpu_ray_sensor.so">
<namespace>${namespace}</namespace>
<input_topic>${topic}</input_topic>
</plugin> -->
</sensor>
</gazebo>
</xacro:macro>
<xacro:macro name="forward_looking_sonar"
params="namespace suffix parent_link topic mass update_rate
samples fov width height *inertia *origin">
<!-- Sensor link -->
<link name="${namespace}/forward_sonar${suffix}_link">
<inertial>
<xacro:insert_block name="inertia" />
<mass value="${mass}" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
<visual>
<geometry>
<mesh filename="file://$(find uuv_sensor_ros_plugins)/meshes/oe14-372.dae" scale="1 1 1"/>
</geometry>
</visual>
<xacro:no_collision/>
</link>
<joint name="${namespace}_forward_sonar${suffix}_joint" type="revolute">
<xacro:insert_block name="origin" />
<parent link="${parent_link}" />
<child link="${namespace}/forward_sonar${suffix}_link" />
<limit upper="0" lower="0" effort="0" velocity="0" />
<axis xyz="1 0 0"/>
</joint>
<gazebo reference="${namespace}/forward_sonar${suffix}_link">
<sensor name="${namespace}/image_sonar" type="depth">
<camera>
<horizontal_fov>${fov}</horizontal_fov>
<image>
<width>${width}</width>
<height>${height}</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>17</far>
</clip>
<save enabled="true">
<path>/tmp/camera</path>
</save>
</camera>
<plugin filename="libimage_sonar_ros_plugin.so" name="forward_sonar${suffix}_controller">
<topicName>${topic}</topicName>
<frameName>forward_sonar${suffix}_optical_frame</frameName>
</plugin>
<always_on>true</always_on>
<update_rate>${update_rate}</update_rate>
</sensor>
</gazebo>
<joint name="${namespace}/forward_sonar${suffix}_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
<parent link="${namespace}/forward_sonar${suffix}_link"/>
<child link="${namespace}/forward_sonar${suffix}_optical_frame"/>
</joint>
<link name="${namespace}/forward_sonar${suffix}_optical_frame"/>
</xacro:macro>
</robot>
首先出现的问题就是理清这个代码中间不同的声纳之间的关系，以及多波束声呐应该加入在那一部分中
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Generates a link element with Blueview P900 physical & visual characteristics -->
<xacro:macro name="multibeam_blueview_p900_link" params="name scale">
<link name="${name}_link">
<inertial>
<mass value="4.35"/>
<pose xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="0.0360" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.0360"/>
</inertial>
<visual>
<geometry>
<mesh filename="model://blueview_p900_nps_multibeam/meshes/p900.dae" scale="${scale}"/>
</geometry>
<pose xyz="0 0 0" rpy="0 0 0"/>
</visual>
<collision>
<geometry>
<mesh filename="model://blueview_p900_nps_multibeam/meshes/COLLISION-p900.dae" scale="${scale}"/>
</geometry>
<pose xyz="0 0 0" rpy="0 0 0"/>
</collision>
</link>
</xacro:macro>
<!-- Generates a sensor element with Blueview P900 parameters -->
<!-- 光栅版本 -->
<xacro:macro name="multibeam_blueview_p900_raster_sensor"
params="name sensor_link sonar_image_topic sonar_image_raw_topic debug
maxDistance raySkips plotScaler sensorGain writeLog writeFrameInterval
artificialVehicleVibration constantReflectivity reflectivityDatabaseFile">
<gazebo reference="${sensor_link}">
<sensor name="${name}_sensor" type="depth">
<visualize>true</visualize>
<always_on>true</always_on>
<update_rate>10</update_rate>
<camera>
<!-- 90 degrees for the M900-90 -->
<horizontal_fov>1.57079632679</horizontal_fov>
<image>
<width>512</width>
<!-- Set vertical FOV by setting image height -->
<!-- Approx 2 times the spec sheet (sepc: 20 deg.) -->
<height>228</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.2</near> <!-- optimal 2m-60m -->
<far>${maxDistance + 2}</far>
</clip>
<depth_camera>
<output>${name}_depths</output>
</depth_camera>
<!-- <save enabled="true">
<path>/tmp/camera</path>
</save> -->
</camera>
<plugin filename="libnps_multibeam_sonar_ros_plugin.so" name="${name}_sensor_controller">
<!-- Sonar properties -->
<verticalFOV>20</verticalFOV>
<sonarFreq>900e3</sonarFreq>
<bandwidth>29.9e3</bandwidth>
<soundSpeed>1500</soundSpeed>
<sourceLevel>220</sourceLevel>
<maxDistance>${maxDistance}</maxDistance>
<constantReflectivity>${constantReflectivity}</constantReflectivity>
<!-- The CSV databsefile is located at the worlds folder -->
<reflectivityDatabaseFile>${reflectivityDatabaseFile}</reflectivityDatabaseFile>
<raySkips>${raySkips}</raySkips>
<sensorGain>${sensorGain}</sensorGain>
<plotScaler>${plotScaler}</plotScaler>
<writeLog>${writeLog}</writeLog>
<debugFlag>${debug}</debugFlag>
<writeFrameInterval>${writeFrameInterval}</writeFrameInterval>
<artificialVehicleVibration>${artificialVehicleVibration}</artificialVehicleVibration>
<!-- This name is prepended to ROS topics -->
<cameraName>${name}</cameraName>
<!-- ROS publication topics -->
<imageTopicName>${name}/image_raw</imageTopicName>
<cameraInfoTopicName>${name}/image_raw/camera_info</cameraInfoTopicName>
<pointCloudTopicName>${name}/point_cloud</pointCloudTopicName>
<depthImageTopicName>${name}/image_depth</depthImageTopicName>
<depthImageCameraInfoTopicName>${name}/image_depth/camera_info</depthImageCameraInfoTopicName>
<sonarImageRawTopicName>${sonar_image_raw_topic}</sonarImageRawTopicName>
<sonarImageTopicName>${sonar_image_topic}</sonarImageTopicName>
<frameName>${name}_forward_sonar_optical_link</frameName>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
<!-- GPU ray版本 -->
<xacro:macro name="multibeam_blueview_p900_ray_sensor"
params="name pcl_topic_name sensor_link sonar_image_topic sonar_image_raw_topic
maxDistance raySkips plotScaler sensorGain writeLog writeFrameInterval
gaussian_noise_mean gaussian_noise_stddev fidelity debug">
<gazebo reference="${sensor_link}">
<sensor name="${name}_sensor" type="gpu_ray">
<visualize>false</visualize>
<always_on>true</always_on>
<update_rate>10</update_rate>
<ray>
<scan>
<!-- Set horizontal FOV by setting image height -->
<!-- 512 beams with 90 degrees horizontal FOV (M900-90) -->
<horizontal>
<samples>512</samples>
<resolution>1</resolution>
<min_angle>-0.78539816339</min_angle>
<max_angle>0.78539816339</max_angle>
</horizontal>
<!-- Set vertical FOV by setting image height -->
<!-- Approx 2 times the spec sheet (sepc: 20 deg.) -->
<vertical>
<samples>${fidelity}</samples>
<resolution>1</resolution>
<min_angle>-0.349066</min_angle>
<max_angle>0.349066</max_angle>
</vertical>
</scan>
<range>
<!-- min range is a collision range no need to change-->
<min>0.1</min>
<max>${maxDistance + 2}</max>
<resolution>0.1</resolution>
</range>
<noise>
<type>gaussian</type>
<!-- Noise parameters based on published spec for Hokuyo laser
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
reading. -->
<mean>${gaussian_noise_mean}</mean>
<stddev>${gaussian_noise_stddev}</stddev>
</noise>
</ray>
<plugin name="${name}_ray_sonar_sensor" filename="libgazebo_ros_velodyne_gpu_laser.so">
<topicName>${name}_point_cloud</topicName>
<!-- <frameName>${name}_forward_sonar_optical_link</frameName> -->
<organize_cloud>true</organize_cloud>
<min_range>0.2</min_range>
<max_range>${maxDistance}</max_range>
</plugin>
<plugin filename="libnps_multibeam_sonar_ray_ros_plugin.so" name="${name}_sensor_controller">
<!-- Sonar properties -->
<verticalFOV>20</verticalFOV>
<sonarFreq>900e3</sonarFreq>
<bandwidth>29.9e3</bandwidth>
<soundSpeed>1500</soundSpeed>
<sourceLevel>220</sourceLevel>
<maxDistance>${maxDistance}</maxDistance>
<raySkips>${raySkips}</raySkips>
<sensorGain>${sensorGain}</sensorGain>
<plotScaler>${plotScaler}</plotScaler>
<writeLog>${writeLog}</writeLog>
<debugFlag>${debug}</debugFlag>
<writeFrameInterval>${writeFrameInterval}</writeFrameInterval>
<!-- This name is prepended to ROS topics -->
<cameraName>${name}</cameraName>
<!-- ROS publication topics -->
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>image_raw/camera_info</cameraInfoTopicName>
<pointCloudTopicName>${pcl_topic_name}</pointCloudTopicName>
<sonarImageRawTopicName>${sonar_image_raw_topic}</sonarImageRawTopicName>
<sonarImageTopicName>${sonar_image_topic}</sonarImageTopicName>
<frameName>${name}_forward_sonar_optical_link</frameName>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
<xacro:macro name="multibeam_sonar_visual_link" params="name">
<link name="${name}_visual_ray_link">
<inertial>
<mass value="0.00001"/>
<pose xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="0.00000000017" ixy="0" ixz="0" iyy="0.00000000017" iyz="0" izz="0.00000000017"/>
</inertial>
</link>
</xacro:macro>
<xacro:macro name="multibeam_sonar_visual_joint"
params="name">
<joint name="ray_joint" type="fixed">
<pose xyz="0 0 0" rpy="0 0 0"/>
<parent link="${name}_link"/>
<child link="${name}_visual_ray_link"/>
</joint>
</xacro:macro>
<xacro:macro name="multibeam_blueview_p900_visual_ray" params="name ray_visual">
<gazebo reference="${name}_visual_ray_link">
<sensor type="ray" name="multibeam_ray">
<pose>0 0 0 0 0 0</pose>
<update_rate>5</update_rate>
<visualize>${ray_visual}</visualize>
<ray>
<scan>
<horizontal>
<samples>100</samples>
<resolution>1</resolution>
<min_angle>-0.785398</min_angle>
<max_angle>0.785398</max_angle>
</horizontal>
<vertical>
<samples>3</samples>
<resolution>1</resolution>
<min_angle>-0.174533</min_angle>
<max_angle>0.174533</max_angle>
</vertical>
</scan>
<range>
<min>0.2</min>
<max>60</max>
<resolution>0.01</resolution>
</range>
</ray>
</sensor>
</gazebo>
</xacro:macro>
<xacro:arg name="standalone" default="false"/>
<xacro:arg name="gpu_ray" default="false"/>
<xacro:arg name="name" default="blueview_p900"/>
<xacro:arg name="maxDistance" default="10"/>
<xacro:arg name="raySkips" default="10"/>
<xacro:arg name="scale" default="1 1 1"/>
<xacro:arg name="sonar_image_topic" default="sonar_image"/>
<xacro:arg name="sonar_image_raw_topic" default="sonar_image_raw"/>
<xacro:arg name="ray_visual" default="true"/>
<xacro:arg name="plotScaler" default="0"/>
<xacro:arg name="sensorGain" default="0.02"/>
<xacro:arg name="writeLog" default="false"/>
<xacro:arg name="debug" default="false"/>
<xacro:arg name="writeFrameInterval" default="5"/>
<xacro:arg name="artificialVehicleVibration" default="false"/>
<xacro:arg name="constantReflectivity" default="true"/>
<xacro:arg name="reflectivityDatabaseFile" default="variationalReflectivityDatabase.csv"/>
<xacro:arg name="gaussian_noise_mean" default="0.0"/>
<xacro:arg name="gaussian_noise_stddev" default="0.0"/>
<xacro:arg name="fidelity" default="500"/>
<xacro:arg name="robot_name" default=""/>
<!-- Top level macro for generating the link, sensor, joint combo
The macro will create the link, joint, and sensor for the DVL.
The DVL can be attached anywhere on the robot (joint_xyz parameter)
but sensor orientation will always be forward and down relative to
the robot link (robot_link parameter). -->
<!-- 这部分是将传感器连接到机器人上的部分 -->
<xacro:macro name="multibeam_blueview_p900_raster_macro"
params="name scale sonar_image_topic debug
sonar_image_raw_topic maxDistance raySkips plotScaler sensorGain writeLog
writeFrameInterval artificialVehicleVibration constantReflectivity
reflectivityDatabaseFile robot_link ray_visual">
<xacro:multibeam_blueview_p900_link name="${name}" scale="${scale}"/>
<joint name="multibeam_sonar_joint" type="fixed">
<parent link="${robot_link}"/>
<child link="${name}_link"/>
<xacro:insert_block name="origin"/>\
</joint>
<xacro:multibeam_blueview_p900_raster_sensor name="${name}" debug="${debug}"
sonar_image_topic="${sonar_image_topic}" sensor_link="${name}_link"
sonar_image_raw_topic="${sonar_image_raw_topic}"
maxDistance="${maxDistance}" raySkips="${raySkips}"
plotScaler="${plotScaler}" sensorGain="${sensorGain}"
writeLog="${writeLog}" writeFrameInterval="${writeFrameInterval}"
artificialVehicleVibration="${artificialVehicleVibration}"
constantReflectivity="${constantReflectivity}"
reflectivityDatabaseFile="${reflectivityDatabaseFile}"/>
<!-- Ray Visual -->
<xacro:multibeam_sonar_visual_link name="${name}"/>
<xacro:multibeam_sonar_visual_joint name="${name}"/>
<xacro:multibeam_blueview_p900_visual_ray name="${name}" ray_visual="${ray_visual}"/>
</xacro:macro>
<xacro:macro name="multibeam_blueview_p900_ray_macro"
params="name scale sonar_image_topic debug robot_name
sonar_image_raw_topic maxDistance raySkips plotScaler sensorGain writeLog
writeFrameInterval gaussian_noise_mean gaussian_noise_stddev fidelity
robot_link ray_visual *origin">
<xacro:multibeam_blueview_p900_link name="${name}" scale="${scale}"/>
<joint name="multibeam_sonar_joint" type="fixed">
<parent link="${robot_link}"/>
<child link="${name}_link"/>
<xacro:insert_block name="origin"/>\
</joint>
<xacro:multibeam_blueview_p900_ray_sensor name="${name}" sensor_link="${name}_link"
sonar_image_topic="${sonar_image_topic}" debug="${debug}"
sonar_image_raw_topic="${sonar_image_raw_topic}"
maxDistance="${maxDistance}" raySkips="${raySkips}"
plotScaler="${plotScaler}" sensorGain="${sensorGain}"
writeLog="${writeLog}" writeFrameInterval="${writeFrameInterval}"
gaussian_noise_mean="${gaussian_noise_mean}"
gaussian_noise_stddev="${gaussian_noise_stddev}"
fidelity="${fidelity}" pcl_topic_name="${robot_name}/${name}_point_cloud"/>
<!-- Ray Visual -->
<xacro:multibeam_sonar_visual_link name="${name}"/>
<xacro:multibeam_sonar_visual_joint name="${name}"/>
<xacro:multibeam_blueview_p900_visual_ray name="${name}" ray_visual="${ray_visual}"/>
</xacro:macro>
<xacro:macro name="multibeam_blueview_p900_macro"
params="name scale sonar_image_topic debug gpu_ray robot_name
sonar_image_raw_topic maxDistance raySkips plotScaler sensorGain writeLog
writeFrameInterval gaussian_noise_mean gaussian_noise_stddev fidelity
artificialVehicleVibration constantReflectivity reflectivityDatabaseFile
robot_link ray_visual *origin">
<xacro:multibeam_blueview_p900_link name="${name}" scale="${scale}"/>
<joint name="multibeam_sonar_joint" type="fixed">
<parent link="${robot_link}"/>
<child link="${name}_link"/>
<xacro:insert_block name="origin"/>
</joint>
<xacro:if value="$(arg gpu_ray)">
<xacro:multibeam_blueview_p900_ray_sensor name="${name}" sensor_link="${name}_link"
sonar_image_topic="${sonar_image_topic}" debug="${debug}"
sonar_image_raw_topic="${sonar_image_raw_topic}"
maxDistance="${maxDistance}" raySkips="${raySkips}"
plotScaler="${plotScaler}" sensorGain="${sensorGain}"
writeLog="${writeLog}" writeFrameInterval="${writeFrameInterval}"
gaussian_noise_mean="${gaussian_noise_mean}"
gaussian_noise_stddev="${gaussian_noise_stddev}"
fidelity="${fidelity}" pcl_topic_name="${robot_name}/${name}_point_cloud"/>
</xacro:if>
<xacro:unless value="$(arg gpu_ray)">
<xacro:multibeam_blueview_p900_raster_sensor name="${name}" debug="${debug}"
sonar_image_topic="${sonar_image_topic}" sensor_link="${name}_link"
sonar_image_raw_topic="${sonar_image_raw_topic}"
maxDistance="${maxDistance}" raySkips="${raySkips}"
plotScaler="${plotScaler}" sensorGain="${sensorGain}"
writeLog="${writeLog}" writeFrameInterval="${writeFrameInterval}"
artificialVehicleVibration="${artificialVehicleVibration}"
constantReflectivity="${constantReflectivity}"
reflectivityDatabaseFile="${reflectivityDatabaseFile}"/>
</xacro:unless>
<!-- Ray Visual -->
<xacro:multibeam_sonar_visual_link name="${name}"/>
<xacro:multibeam_sonar_visual_joint name="${name}"/>
<xacro:multibeam_blueview_p900_visual_ray name="${name}" ray_visual="${ray_visual}"/>
</xacro:macro>
<!-- For standalone -->
<xacro:macro name="multibeam_sonar_standalone_link" params="name">
<link name="${name}_standalone_base_link"/>
<gazebo>
<static>true</static>
</gazebo>
</xacro:macro>
<xacro:macro name="multibeam_blueview_p900_raster_standalone_macro"
params="name scale sonar_image_topic debug
sonar_image_raw_topic maxDistance raySkips plotScaler sensorGain writeLog
writeFrameInterval artificialVehicleVibration constantReflectivity
reflectivityDatabaseFile ray_visual">
<xacro:multibeam_sonar_standalone_link name="${name}"/>
<xacro:multibeam_blueview_p900_link name="${name}" scale="${scale}"/>
<joint name="multibeam_sonar_joint" type="fixed">
<parent link="${name}_standalone_base_link"/>
<child link="${name}_link"/>
</joint>
<xacro:multibeam_blueview_p900_raster_sensor name="${name}" sensor_link="${name}_link"
sonar_image_topic="${sonar_image_topic}" debug="${debug}"
sonar_image_raw_topic="${sonar_image_raw_topic}"
maxDistance="${maxDistance}" raySkips="${raySkips}"
plotScaler="${plotScaler}" sensorGain="${sensorGain}"
writeLog="${writeLog}" writeFrameInterval="${writeFrameInterval}"
artificialVehicleVibration="${artificialVehicleVibration}"
constantReflectivity="${constantReflectivity}"
reflectivityDatabaseFile="${reflectivityDatabaseFile}"/>
<!-- Ray Visual -->
<xacro:multibeam_sonar_visual_link name="${name}"/>
<xacro:multibeam_sonar_visual_joint name="${name}"/>
<xacro:multibeam_blueview_p900_visual_ray name="${name}" ray_visual="${ray_visual}" />
</xacro:macro>
<xacro:macro name="multibeam_blueview_p900_ray_standalone_macro"
params="name scale sonar_image_topic fidelity debug robot_name
sonar_image_raw_topic maxDistance raySkips plotScaler sensorGain writeLog
writeFrameInterval gaussian_noise_mean gaussian_noise_stddev ray_visual">
<xacro:multibeam_sonar_standalone_link name="${name}"/>
<xacro:multibeam_blueview_p900_link name="${name}" scale="${scale}"/>
<joint name="multibeam_sonar_joint" type="fixed">
<parent link="${name}_standalone_base_link"/>
<child link="${name}_link"/>
</joint>
<xacro:multibeam_blueview_p900_ray_sensor name="${name}" sensor_link="${name}_link"
sonar_image_topic="${sonar_image_topic}" debug="${debug}"
sonar_image_raw_topic="${sonar_image_raw_topic}"
maxDistance="${maxDistance}" raySkips="${raySkips}"
plotScaler="${plotScaler}" sensorGain="${sensorGain}"
writeLog="${writeLog}" writeFrameInterval="${writeFrameInterval}"
gaussian_noise_mean="${gaussian_noise_mean}"
gaussian_noise_stddev="${gaussian_noise_stddev}"
fidelity="${fidelity}" pcl_topic_name="${name}_point_cloud"/>
<!-- Ray Visual -->
<xacro:multibeam_sonar_visual_link name="${name}"/>
<xacro:multibeam_sonar_visual_joint name="${name}"/>
<xacro:multibeam_blueview_p900_visual_ray name="${name}" ray_visual="${ray_visual}" />
</xacro:macro>
<xacro:if value="$(arg standalone)">
<xacro:unless value="$(arg gpu_ray)">
<xacro:multibeam_blueview_p900_raster_standalone_macro
name="$(arg name)" scale="$(arg scale)" debug="$(arg debug)"
sonar_image_topic="$(arg sonar_image_topic)"
sonar_image_raw_topic="$(arg sonar_image_raw_topic)"
maxDistance="$(arg maxDistance)" raySkips="$(arg raySkips)"
plotScaler="$(arg plotScaler)" sensorGain="$(arg sensorGain)"
writeLog="$(arg writeLog)" writeFrameInterval="$(arg writeFrameInterval)"
artificialVehicleVibration="$(arg artificialVehicleVibration)"
constantReflectivity="$(arg constantReflectivity)"
reflectivityDatabaseFile="$(arg reflectivityDatabaseFile)"
ray_visual="$(arg ray_visual)"/>
</xacro:unless>
<xacro:if value="$(arg gpu_ray)">
<xacro:multibeam_blueview_p900_ray_standalone_macro
name="$(arg name)" scale="$(arg scale)" debug="$(arg debug)"
sonar_image_topic="$(arg sonar_image_topic)"
sonar_image_raw_topic="$(arg sonar_image_raw_topic)"
maxDistance="$(arg maxDistance)" raySkips="$(arg raySkips)"
plotScaler="$(arg plotScaler)" sensorGain="$(arg sensorGain)"
writeLog="$(arg writeLog)" writeFrameInterval="$(arg writeFrameInterval)"
gaussian_noise_mean="$(arg gaussian_noise_mean)"
gaussian_noise_stddev="$(arg gaussian_noise_stddev)"
fidelity="$(arg fidelity)" robot_name=""
ray_visual="$(arg ray_visual)"/>
</xacro:if>
</xacro:if>
</robot>尝试一：将整个p900的xacro文件添加到sonar_snippets中，然后在sensors_snippets中调用其中的声纳函数，失败（undefined params参数没有定义——我觉得这个还可以进一步修改，比如提前声明参数，但是由于各种文件包含太复杂，这个参数的声明具体怎么整，整到哪里，都有问题）
尝试二：运行项目中的sonar.launch文件，出现错误，发现rviz中的机器人模型有错误（Unable to call parseURDF on robot model），查找后应该是URDF文件解析的问题，搞不明白（超级错误的一个东西，建议直接pass）
尝试三：使用joint将机器人模型和声纳连接起来，这个主要问题是joint的parent和link
今天完全搞清楚了代码的结构，明天尝试替换掉parent_link看能不能直接加载到机器人上
好了！！
新的问题：origin的哪个insert_block函数是那里的？（目前看来这个问题没有影响）
现在能够显示出来声纳
新的问题：
1、这个声呐连接在了base_link上，但是其他的传感器都有自己的link这里也要查看原因并且修改//这周要解决这个问题
2、在rviz中，声呐的话题应该是出现了坐标变换的问题，要学习如何修改，错误如下：
Transform [sender=unknown_publisher]
For frame [blueview_p900_forward_sonar_optical_link]: Frame [blueview_p900_forward_sonar_optical_link] does not exist
3、要修改launch文件，然后实现以前的功能
4、水下环境的搭建
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
