Laser Scanner Gazebo Plugin Crashes on Contact
Hello,
I have the attached urdf and xacro files for a small differential drive robot. I am able to launch the model in rviz, see the laser scanner beams and get the topic. However when any other model touches the beams gazebo segfaults with:
Thread 35 "gzserver" received signal SIGSEGV, Segmentation fault. [Switching to Thread 0x7fff597b9700 (LWP 29332)] 0x00007ffff684ca38 in gazebo::sensors::Noise::Apply(double) () from /usr/lib/x8664-linux-gnu/libgazebosensors.so.7
Any ideas on what could be the issue? I tried disabling GPU acceleration but the problem persists.
Any help would be appreciated. For reference I am running Kinetic on Ubuntu 16.04 with Gazebo 7
Robot xacro file
<?xml version="1.0"?>
<!-- PROPERTY LIST -->
<!--All units in m-kg-s-radians unit system -->
<xacro:property name="M_PI" value="3.1415926535897931"/>
<xacro:property name="footprint_vertical_offset" value="-0.05"/>
<!-- Chassis -->
<xacro:property name="base_height" value="0.12"/>
<xacro:property name="base_width" value="0.195"/>
<xacro:property name="base_length" value="0.29"/>
<xacro:property name="base_mass" value="1"/> <!-- in kg-->
<xacro:property name="wheel_mass" value="1"/> <!-- in kg-->
<!-- Wheels -->
<xacro:property name="wheel_horizontal_separation" value="0.075"/>
<xacro:property name="wheel_vertical_separation" value="0.11"/>
<xacro:property name="wheel_vertical_offset" value="-0.13"/>
<xacro:property name="wheel_radius" value="0.05"/>
<xacro:property name="wheel_width" value="0.045"/>
<!-- BASE-FOOTPRINT -->
<!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin -->
<link name="base_footprint">
<inertial>
<mass value="0.0001"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<gazebo reference="base_footprint">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="base_link_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_footprint"/>
<child link="base_link"/>
</joint>
<link name="base_link">
<visual>
<origin xyz="0 0 ${footprint_vertical_offset}" rpy="0 0 0"/>
<geometry>
<!--<mesh filename="package://simple_robot_description_4wheels/meshes/inno_sim.stl"/>-->
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
<material name="Black"/>
</visual>
<collision>
<origin xyz="0 0 ${footprint_vertical_offset}"/>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</collision>
<inertial>
<!-- Center of mass -->
<origin xyz="0.012 0.002 0.067" rpy="0 0 0"/>
<!--<mass value="16.523"/>-->
<mass value="5.523"/>
<!-- Moments of inertia: ( base without wheels ) -->
<inertia
ixx="0.3136" ixy="-0.0008" ixz="0.0164"
iyy="0.3922" iyz="-0.0009"
izz="0.4485"/>
</inertial>
</link>
<gazebo reference="base_link">
<material>Gazebo/Black</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<!-- Start of Wheels
-->
<xacro:macro name="wheel" params="prefix trans_x trans_y trans_z">
<link name="${prefix}_wheel">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
<material name="Red"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.477"/>
<inertia
ixx="0.0013" ixy="0" ixz="0"
iyy="0.0024" iyz="0"
izz="0.0013"/>
</inertial>
</link>
<gazebo reference="${prefix}_wheel">
<!--<mu1>10</mu1>-->
<!--<mu2>10</mu2>-->
<material>Gazebo/Red</material>
</gazebo>
<joint name="${prefix}_joint" type="continuous">
<parent link="base_link"/>
<child link="${prefix}_wheel"/>
<origin xyz="${trans_x} ${trans_y} ${trans_z}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<transmission name="${prefix}_transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}_wheel_joint">
<!--<hardwareInterface>VelocityJointInterface</hardwareInterface>-->
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}_motor">
<!--<hardwareInterface>VelocityJointInterface</hardwareInterface>-->
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:wheel prefix="front_left" trans_x="${wheel_horizontal_separation}"
trans_y="${wheel_vertical_separation+wheel_width/2}" trans_z="${wheel_vertical_offset}"/>
<xacro:wheel prefix="front_right" trans_x="${wheel_horizontal_separation}"
trans_y="${-wheel_vertical_separation-wheel_width/2}" trans_z="${wheel_vertical_offset}"/>
<xacro:wheel prefix="rear_left" trans_x="${-wheel_horizontal_separation}"
trans_y="${wheel_vertical_separation+wheel_width/2}" trans_z="${wheel_vertical_offset}"/>
<xacro:wheel prefix="rear_right" trans_x="${-wheel_horizontal_separation}"
trans_y="${-wheel_vertical_separation-wheel_width/2}" trans_z="${wheel_vertical_offset}"/>
<!--<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">-->
<!--<updateRate>100.0</updateRate>-->
<!--<robotNamespace>/</robotNamespace>-->
<!--<leftFrontJoint>front_left_wheel</leftFrontJoint>-->
<!--<rightFrontJoint>front_right_wheel</rightFrontJoint>-->
<!--<leftRearJoint>back_left_wheel</leftRearJoint>-->
<!--<rightRearJoint>back_right_wheel</rightRearJoint>-->
<!--<wheelSeparation>${base_width}</wheelSeparation>-->
<!--<wheelDiameter>${2*wheel_radius}</wheelDiameter>-->
<!--<robotBaseFrame>base_link</robotBaseFrame>-->
<!--<torque>20</torque>-->
<!--<topicName>cmd_vel</topicName>-->
<!--<broadcastTF>false</broadcastTF>-->
<!--</plugin>-->
<!-- End of Wheels
-->
<!-- IMU -->
<joint name="imu_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="imu_link"/>
</joint>
<link name="imu_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
<material name="White"/>
</visual>
<inertial>
<!--<mass value="1e-5"/>-->
<mass value="0"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<!--<gazebo reference="imu_link">-->
<!--<material>Gazebo/DarkGrey</material>-->
<!--<turnGravityOff>false</turnGravityOff>-->
<!--</gazebo>-->
<!-- Laser scanner
-->
<joint name="hokuyo_joint" type="fixed">
<origin xyz="0.125 0 0.048" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="hokuyo_link"/>
</joint>
<!-- Hokuyo Laser -->
<link name="hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.045 0.045 0.075"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<!--<box size="0.045 0.045 0.075"/>-->
<mesh filename="package://simple_robot_description_4wheels/meshes/hokuyo.dae"/>
</geometry>
<material name="Grey"/>
</visual>
<inertial>
<mass value="1e-5"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<!--<gazebo reference="hokuyo_link">-->
<!--<material>Gazebo/DarkGrey</material>-->
<!--<turnGravityOff>false</turnGravityOff>-->
<!--</gazebo>-->
<!-- Bring in simulation data for Gazebo. -->
<xacro:include filename="$(find simple_robot_description_4wheels)/urdf/robot_4wheels.gazebo"/>
Gazebo plugins file:
<?xml version="1.0"?>
<!--Move wheels custom behaviour-->
<gazebo>
<plugin name="move_wheels" filename="libmoveWheels_plugin.so">
</plugin>
</gazebo>
<!--Get encoders ticks behaviour-->
<gazebo>
<plugin name="encoders_ticks" filename="libencodersTicks_plugin.so">
</plugin>
</gazebo>
<!--Get Odometry-->
<gazebo>
<plugin name="get_odom" filename="libodom_plugin.so">
<frontLeftJoint>front_left_joint</frontLeftJoint>
<frontRightJoint>front_right_joint</frontRightJoint>
<wheelSeparation>0.46</wheelSeparation>
<wheelDiameter>0.196</wheelDiameter>
</plugin>
</gazebo>
<!-- Defining the colors used in this robot -->
<material name="Black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="Red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<material name="Blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="Grey">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
<gazebo reference="hokuyo_link">
<sensor type="ray" name="laser">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>640</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>6.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>Gaussian</type>
<mean>0.01</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="laser" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>base_laser_link</frameName>
</plugin>
</sensor>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<material>Gazebo/DarkGrey</material>
<sensor name="imu_sensor" type="imu">
<update_rate>10</update_rate>
<plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
<robotNamespace></robotNamespace>
<topicName>imu/data</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>10.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset> <!-- This doesn't do what I expect -->
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<legacyMode>true</legacyMode>
<alwaysOn>true</alwaysOn>
<updateRate>10</updateRate>
<leftJoint>rear_left_joint</leftJoint>
<rightJoint>front_right_joint</rightJoint>
<wheelSeparation>${base_width}</wheelSeparation>
<wheelDiameter>${2*wheel_radius}</wheelDiameter>
<wheelTorque>5</wheelTorque>
<robotBaseFrame>base_link</robotBaseFrame>
<torque>5</torque>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<publishWheelTF>true</publishWheelTF>
<publishWheelJointState>true</publishWheelJointState>
</plugin>
</gazebo>
Asked by AndreasLydakis on 2018-06-27 09:44:56 UTC
Comments