urdf model (depth data is being shown in the wrong place in Rviz and non fixed joint not published) - ros

I am a beginner in ROS and related issues. I create a URDF model of my desired robot that has a depth camera. I have 2 questions:
Robot simulated correctly in gazebo but when I add some interest object in gazebo environment, I found that my camera sensor frame is wrong and it view direction is z axis of the robot. I read some related problem but I could fix it.
I use of plugin for skid driver, but my robot wheels' not published in rviz, when I checked robot model or rqt_garph I could see the wheels.
Could you please help me regarding them?
It is my urdf:
<?xml version="1.0"?>
<robot name="bluebot" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- base information-->
<xacro:property name="base_length" value="0.50" />
<xacro:property name="base_width" value="0.40" />
<xacro:property name="base_height" value="0.10" />
<xacro:property name="base_mass" value="14" />
<!-- wheel information-->
<xacro:property name="wheel_length" value="0.08" />
<xacro:property name="wheel_radius" value="0.10" />
<xacro:property name="wheel_mass" value="4.70" />
<!-- camera_stand information-->
<xacro:property name="camera_stand_length" value="0.10" />
<xacro:property name="camera_stand_radius" value="0.05" />
<xacro:property name="camera_stand_mass" value="2.50" />
<!-- camera information-->
<xacro:property name="camera_length" value="0.15" />
<xacro:property name="camera_width" value="0.03" />
<xacro:property name="camera_height" value="0.02" />
<xacro:property name="camera_mass" value="1" />
<xacro:property name="Pi" value="3.1415" />
<!--inertial-->
<xacro:macro name="solid_cuboid_inertial" params="length width height mass">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${(mass / 12) * ((base_height * base_height) + (base_length * base_length))}" ixy="0.0" ixz="0.0"
iyy="${(mass / 12) * ((base_width * base_width) + (base_length * base_length))}" iyz="0.0"
izz="${(mass / 12) * ((base_width * base_width) + (base_height * base_height))}"/>
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial" params="mass radius height">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${(mass / 12) * ((3 * (radius * radius)) + ( height * height))}" ixy="0.0" ixz="0.0"
iyy="${(mass / 12) * ((3 * ( radius* radius)) + ( height * height))}" iyz="0.0"
izz="${(mass / 12) * (radius * radius)}"/>
</inertial>
</xacro:macro>
<!--base_link-->
<link name="base_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 ${base_height / 2}"/>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</collision>
<xacro:solid_cuboid_inertial length="${base_length}" width="${base_width}" height="${base_height}" mass="${base_mass}"/>
</link>
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>
<!--wheel-->
<xacro:macro name="wheel" params="name reflect1 reflect2">
<link name="${name}_wheel_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length ="${wheel_length}" radius="${wheel_radius}"/>
</geometry>
</visual>
<collision>
<geometry>
<cylinder length="${wheel_length}" radius="${wheel_radius}"/>
</geometry>
</collision>
<xacro:cylinder_inertial mass="${wheel_mass}" radius="${wheel_radius}" height="${wheel_length}" />
</link>
<gazebo reference="${name}_wheel_link">
<material>Gazebo/Gray</material>
<mu1 value="10.0"/>
<mu2 value="20.0"/>
</gazebo>
<joint name="${name}_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="${name}_wheel_link"/>
<origin rpy="${Pi / 2} 0 0" xyz="${reflect2 * ((base_width / 2) + (wheel_length / 2))} ${reflect1 * base_length / 2} 0"/>
<axis xyz="0 0 -1"/>
</joint>
<transmission name="${name}_wheel_transmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="${name}_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="${name}_wheel_joint">
<!-- <hardwareInterface>VelocityJointInterface</hardwareInterface> -->
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
</transmission>
</xacro:macro>
<xacro:wheel name="right_front" reflect1="1" reflect2="1" />
<xacro:wheel name="left_front" reflect1="-1" reflect2="1" />
<xacro:wheel name="right_back" reflect1="1" reflect2="-1" />
<xacro:wheel name="left_back" reflect1="-1" reflect2="-1" />
<!--camera_stand-->
<link name="camera_stand_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="${camera_stand_length}" radius="${camera_stand_radius}"/>
</geometry>
</visual>
<collision>
<geometry>
<cylinder length="${camera_stand_length}" radius="${camera_stand_radius}"/>
</geometry>
</collision>
<xacro:cylinder_inertial mass="${camera_stand_mass}" radius="${camera_stand_radius}" height="${camera_stand_length}" />
</link>
<gazebo reference="camera_stand_link">
<material>Gazebo/Blue</material>
</gazebo>
<joint name="camera_stand_joint" type="fixed">
<parent link="base_link"/>
<child link="camera_stand_link"/>
<origin rpy="0 0 0" xyz="${base_width / 2} 0 ${(base_length / 2) - 0.10}"/>
</joint>
<!--camera-->
<link name="camera_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${camera_length} ${camera_width} ${camera_height}"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="${camera_length} ${camera_width} ${camera_height}"/>
</geometry>
</collision>
<xacro:solid_cuboid_inertial length="${camera_length}" width="${camera_width}" height="${camera_height}" mass="${camera_mass}"/>
</link>
<gazebo reference="camera_link">
<material>Gazebo/Yellow</material>
</gazebo>
<joint name="camera_joint" type="fixed">
<parent link="base_link"/>
<child link="camera_link"/>
<origin rpy="0 0 ${Pi / 2}" xyz="${base_width / 2} 0 ${base_length / 2}"/>
</joint>
<link name="camera_sensor_link"/>
<joint name="camera_sensor_joint" type="fixed">
<parent link="camera_link"/>
<child link="camera_sensor_link"/>
<origin rpy="${-3 * Pi/2} ${Pi/2} ${Pi/2}" xyz="0 0 0"/>
<!-- <origin rpy="${3*Pi/2} 0 ${Pi}" xyz="0 0 0"/> -->
</joint>
<gazebo reference="camera_sensor_link">
<sensor type="depth" name="xtion_pro">
<always_on>true</always_on>
<visualize>true</visualize>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>640</width>===
<height>480</height>
<format>R8G8B8</format>
</image>
<depth_camera>
</depth_camera>
<clip>
<near>0.1</near>
<far>10</far>
</clip>
</camera>
<plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
<baseline>0.2</baseline>
<alwaysOn>true</alwaysOn>
<!-- Keep this zero, update_rate in the parent <sensor> tag
will control the frame rate. -->
<updateRate>0.0</updateRate>
<cameraName>camera_ir</cameraName>
<imageTopicName>/camera/color/image_raw</imageTopicName>
<cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
<depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>/camera/depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
<frameName>camera_sensor_link</frameName>
<pointCloudCutoff>0.5</pointCloudCutoff>
<pointCloudCutoffMax>3.0</pointCloudCutoffMax>
<distortionK1>0</distortionK1>
<distortionK2>0</distortionK2>
<distortionK3>0</distortionK3>
<distortionT1>0</distortionT1>
<distortionT2>0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo>
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace>/</robotNamespace>
<leftFrontJoint>left_front_wheel_joint</leftFrontJoint>
<rightFrontJoint>right_front_wheel_joint</rightFrontJoint>
<leftRearJoint>left_back_wheel_joint</leftRearJoint>
<rightRearJoint>right_back_wheel_joint</rightRearJoint>
<wheelSeparation>0.4</wheelSeparation>
<wheelDiameter>${wheel_radius * 2}</wheelDiameter>
<robotBaseFrame>base_link</robotBaseFrame>
<torque>20</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>false</broadcastTF>
</plugin>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/</robotNamespace>
</plugin>
</gazebo>
</robot>
and it is my launch file:
<?xml version="1.0"?>
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="extra_gazebo_args" default=""/>
<arg name="gui" default="true"/>
<arg name="recording" default="false"/>
<!-- Note that 'headless' is currently non-functional. See gazebo_ros_pkgs issue #491 (-r arg does not disable
rendering, but instead enables recording). The arg definition has been left here to prevent breaking downstream
launch files, but it does nothing. -->
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="physics" default="ode"/>
<arg name="verbose" default="false"/>
<arg name="output" default="screen"/>
<arg name="world_name" default="empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
<arg name="respawn_gazebo" default="false"/>
<arg name="use_clock_frequency" default="false"/>
<arg name="pub_clock_frequency" default="100"/>
<arg name="enable_ros_network" default="true" />
<!-- set use_sim_time flag -->
<param name="/use_sim_time" value="$(arg use_sim_time)"/>
<!-- set command arguments -->
<arg unless="$(arg paused)" name="command_arg1" value=""/>
<arg if="$(arg paused)" name="command_arg1" value="-u"/>
<arg unless="$(arg recording)" name="command_arg2" value=""/>
<arg if="$(arg recording)" name="command_arg2" value="-r"/>
<arg unless="$(arg verbose)" name="command_arg3" value=""/>
<arg if="$(arg verbose)" name="command_arg3" value="--verbose"/>
<arg unless="$(arg debug)" name="script_type" value="gzserver"/>
<arg if="$(arg debug)" name="script_type" value="debug"/>
<!-- start gazebo server-->
<group if="$(arg use_clock_frequency)">
<param name="gazebo/pub_clock_frequency" value="$(arg pub_clock_frequency)" />
</group>
<group>
<param name="gazebo/enable_ros_network" value="$(arg enable_ros_network)" />
</group>
<node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="$(arg respawn_gazebo)" output="$(arg output)"
args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(find bluebot)/worlds/$(arg world_name)" />
<!-- start gazebo client -->
<group if="$(arg gui)">
<node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="$(arg output)" args="$(arg command_arg3)"/>
</group>
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description" command="xacro $(find bluebot)/urdf/bluebot.xacro" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model bluebot" />
<!-- <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen"/> -->
<!-- load the controllers -->
<!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/" args="left_front_wheel_joint_position_controller right_front_wheel_joint_position_controller
left_back_wheel_joint_position_controller right_back_wheel_joint_position_controller joint_state_controller"/> -->
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/bluebot/joint_states" />
</node>
</launch>

Related

Drake Parser error: Unable to read the 'value' attribute for the <drake:hunt_crossley_dissipation> tag

I am trying to write a URDF file for a simple pendulum that will act as a 'paddle' - i.e. upon collision with a bouncy ball, the ball will bounce off (like a volleyball player passing a volleyball).
Inspired by this bouncy ball example, I have written the URDF file given below, but loading it with parser.AddModelFromFile("filename.urdf") gives this error:
RuntimeError: filename.urdf:39: error: Unable to read the 'value' attribute for the <drake:hunt_crossley_dissipation> tag
I am surprised that this doesn't work, since it was adapted from the bouncing ball example (which does work). I would really appreciate any guidance, thanks!
URDF file:
<?xml version="1.0"?>
<robot name="pendulum-paddle">
<material name="black">
<color rgba="0 0 0 1" />
</material>
<link name="base">
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
</link>
<joint name="base_weld" type="fixed">
<parent link="world" />
<child link="base" />
</joint>
<link name="arm">
<inertial>
<origin xyz="0 0 -1" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin xyz="0 0 -.5" rpy="0 0 0" />
<geometry>
<box size="0.2 0.2 1"/>
</geometry>
<material name="black" />
</visual>
<collision name="collision">
<origin xyz="0 0 -.5" rpy="0 0 0" />
<geometry>
<box size="0.2 0.2 1"/>
</geometry>
<drake:proximity_properties>
<drake:hunt_crossley_dissipation>
0.1
</drake:hunt_crossley_dissipation>
<drake:point_contact_stiffness>
1000000
</drake:point_contact_stiffness>
</drake:proximity_properties>
</collision>
</link>
<joint name="shoulder" type="continuous">
<parent link="base"/>
<child link="arm" />
<axis xyz="0 1 0" />
</joint>
<transmission type="SimpleTransmission" name="shoulder_trans">
<actuator name="shoulder" />
<joint name="shoulder" />
<mechanicalReduction>1</mechanicalReduction>
</transmission>
</robot>
Unfortunately, the syntax is URDF is different than in SDF, and we tried to make the Drake tags comply to the standard.
https://drake.mit.edu/doxygen_cxx/group__multibody__parsing.html#tag_drake_hunt_crossley_dissipation
In urdf, the proximity properties look more like:
<drake:proximity_properties>
<drake:mu_static value="0.8"/>
<drake:mu_dynamic value="0.3"/>
</drake:proximity_properties>
so try
<drake:proximity_properties>
<drake:hunt_crossley_dissipation value="0.1"/>
<drake:point_contact_stiffness value="1000000"/>
</drake:proximity_properties>

Gazebo 4 Wheeled Robot does not move

I made a URDF-File for a 4-wheeled Robot. I would like to test it in Gazebo. I have it working so far. I have 4 wheels, they also turn. I confirmed this by turning on View -> Wireframe. I am using the skid_steer_drive_controller and rosrun teleop_twist_keyboard teleop_twist_keyboard.py. I can see the wheels turning but the Robot does not move.
I am using an offset for my base_footprint, otherwise my robot collapses and starts jiggeling. I also took this offset in the z-Axis in account for the wheels.
I took this urdf as basis, here so i did not declare friction:
This is my URDF:
<?xml version="1.0" ?>
<robot name="explorer">
<link name="world">
</link>
<!-- -->
<link name="base_footprint">
<visual>
<origin xyz="0 0 0.046"/>
<geometry>
<box size="0.235 0.094 0.049"/>
</geometry>
<material name="gray">
<color rgba=".2 .2 .2 1" />
</material>
</visual>
<inertial>
<origin xyz="0 0 0.046"/>
<mass value="0.6174" />
<inertia ixx="2.84523645e-3" ixy="0.0" ixz="0" iyy="3.1763172e-3" iyz="0" izz="5.7814365e-4" />
</inertial>
<collision>
<origin xyz="0 0 0.046"/>
<geometry>
<box size="0.235 0.094 0.049"/>
</geometry>
</collision>
</link>
<joint name="dummy_joint" type="fixed">
<parent link="world"/>
<child link="base_footprint"/>
</joint>
<!-- -->
<link name="wheel_rf">
<inertial>
<mass value="0.036" />
<inertia ixx="9.2562e-5" ixy="0.0" ixz="0" iyy="9.2562e-5" iyz="0" izz="2.9124e-5" />
</inertial>
<visual>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
</collision>
</link>
<joint name="wheel_rf_joint" type="continuous">
<origin xyz="0.04 0.063 0.033" rpy="1.57 0.0 0.0"/>
<parent link="base_footprint"/>
<child link="wheel_rf"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<link name="wheel_lf">
<inertial>
<mass value="0.036" />
<inertia ixx="9.2562e-5" ixy="0.0" ixz="0" iyy="9.2562e-5" iyz="0" izz="2.9124e-5" />
</inertial>
<visual>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
</collision>
</link>
<joint name="wheel_lf_joint" type="continuous">
<origin xyz="0.04 -0.063 0.033" rpy="1.57 0.0 0.0"/>
<parent link="base_footprint"/>
<child link="wheel_lf"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- -->
<link name="wheel_rb">
<inertial>
<mass value="0.036" />
<inertia ixx="9.2562e-5" ixy="0.0" ixz="0" iyy="9.2562e-5" iyz="0" izz="2.9124e-5" />
</inertial>
<visual>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
</collision>
</link>
<joint name="wheel_rb_joint" type="continuous">
<origin xyz="-0.04 0.063 0.033" rpy="1.57 0.0 0.0"/>
<parent link="base_footprint"/>
<child link="wheel_rb"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- -->
<link name="wheel_lb">
<inertial>
<mass value="0.036" />
<inertia ixx="9.2562e-5" ixy="0.0" ixz="0" iyy="9.2562e-5" iyz="0" izz="2.9124e-5" />
</inertial>
<visual>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
</collision>
</link>
<joint name="wheel_lb_joint" type="continuous">
<origin xyz="-0.04 -0.063 0.033" rpy="1.57 0.0 0.0"/>
<parent link="base_footprint"/>
<child link="wheel_lb"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- Gazebo colors to links as upper defined colors are only visible in RVIZ-->
<gazebo reference="base_footprint">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="wheel_rf">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="wheel_lf">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="wheel_rb">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="wheel_lb">
<material>Gazebo/Orange</material>
</gazebo>
<!-- DIFFENERNTIAL DRIVEEEEEEEEEEEEE -->
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace>/</robotNamespace>
<leftFrontJoint>wheel_lf_joint</leftFrontJoint>
<rightFrontJoint>wheel_rf_joint</rightFrontJoint>
<leftRearJoint>wheel_lb_joint</leftRearJoint>
<rightRearJoint>wheel_rb_joint</rightRearJoint>
<wheelSeparation>0.08</wheelSeparation>
<wheelDiameter>0.066</wheelDiameter>
<robotBaseFrame>base_footprint</robotBaseFrame>
<torque>20</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>false</broadcastTF>
</plugin>
</gazebo>
</robot>

Camera in Gazebo changes view continuously

I'm trying to attach a simple camera to the tool tip of my robot (Kuka lwr). The problem is that the camera view is always changing as shown in the video:
https://polimi365-my.sharepoint.com/:v:/g/personal/10522502_polimi_it/EXuBinkz9NRJlq9O5L5lSo8B7VvdVAVFOAfmaITn106_4A?e=6i3bw3
It seems like the camera is colliding with something or there is a sort of conflict. At the beginning I though it was a material problem (maybe the camera was set inside the box) but setting it transparent didn't solve the problem. I couldn't find any similar issue on the web so I hope someone can help me! For the camera, I followed the tutorial (http://gazebosim.org/tutorials?tut=ro...) adding the camera description in my .urdf.xacro file and the camera plugin in the .gazebo.xacro file.
Here is the camera description in the xacro file:
<joint name="${name}_joint_camera" type="fixed">
<parent link="tool_ee"/>
<child link="${name}_link_camera"/>
<origin xyz="0 0 0.06" rpy="0 0 0"/>
<axis xyz="1 0 0" />
</joint>
<link name="${name}_link_camera">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<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>
</link>
<joint name="camera_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
<parent link="${name}_link_camera"/>
<child link="camera_link_optical"/>
</joint>
<link name="camera_link_optical">
</link>
Here the camera plugin of in the gazebo file:
<gazebo reference="lwr_link_camera">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Transparent</material>
</gazebo>
<!-- Camera -->
<gazebo reference="lwr_link_camera">
<sensor type="camera" name="camera_sensor1">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link_optical</frameName>
<hackBaseline>0.0</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0.0</Cx>
<Cy>0.0</Cy>
<focalLength>0.0</focalLength>
</plugin>
</sensor>
</gazebo>
Any help is appreciated! Thanks!

Gazebo weird behavior when two objects contact

Hello Gazebo/ROS community on Stack Overflow, I'm trying to simulate the landing of a quadcopter on a pole using Gazebo/ROS Kinetic. However, as soon as the drone contacts the landing pole, it gets bounced away in a very unrealistic way. Both the drone and the pole have their mass, inertia, and friction defined.
A video is avaliable here:
https://youtu.be/8Iis60y5_yk
Can anyone give me a few pointers? Your help is much appreciated!
Here is the .world file:
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<!-- Only one ROS interface plugin is required per world, as any other plugin can connect a Gazebo
topic to a ROS topic (or vise versa). -->
<plugin name="ros_interface_plugin" filename="librotors_gazebo_ros_interface_plugin.so"/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>47.3667</latitude_deg>
<longitude_deg>8.5500</longitude_deg>
<elevation>500.0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<physics type='ode'>
<ode>
<solver>
<type>quick</type>
<iters>1000</iters>
<sor>1.3</sor>
</solver>
<constraints>
<cfm>0</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>100</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</ode>
<max_step_size>0.01</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>100</real_time_update_rate>
<gravity>0 0 -9.8</gravity>
</physics>
</world>
</sdf>
Here is the xacro file for the landing pole:
<?xml version="1.0"?>
<robot name="target" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="pole_height" value="1.2192" />
<xacro:property name="pole_radius" value="0.05" />
<!-- cone-shaped top compatible with the drone -->
<link name="target/cone">
<inertial>
<origin xyz="0 0 0"/>
<mass value="3.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://rotors_description/meshes/cone.dae" scale="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://rotors_description/meshes/cone.dae" scale="0.001 0.001 0.001" />
</geometry>
<contact_coefficients mu ="1" kp="1000" kd="1"/>
</collision>
</link>
<!-- connect cone to pole -->
<joint name="cone_to_pole" type="fixed">
<origin xyz="0 0 0.6096"/>
<parent link="target/base_link"/>
<child link="target/cone"/>
</joint>
<!-- landing pole -->
<link name="target/base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="${pole_height}" radius="${pole_radius}"/>
</geometry>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${pole_height}" radius="${pole_radius}"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0"/>
<mass value="3.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<!-- april tag -->
<link name="target/tag_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="1.7 1.7 0.05" />
</geometry>
<surface>
<friction>
<ode>
<mu>0.01</mu>
<mu2>0.01</mu2>
</ode>
</friction>
</surface>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://rotors_description/meshes/pole.dae" scale="1 1 1" />
</geometry>
</visual>
</link>
<!-- connect pole to tag -->
<joint name="target/tag_joint" type="fixed">
<origin xyz="0 0 -0.6096"/>
<parent link="target/base_link"/>
<child link="target/tag_link"/>
</joint>
<!-- make the landing pole transparent for testing purposes -->
<!-- <gazebo reference="target/base_link">
<visual>
<material>
<ambient> 1.0 1.0 1.0 0.0</ambient>
<diffuse> 1.0 1.0 1.0 0.0</diffuse>
<specular>1.0 1.0 1.0 0.0</specular>
</material>
</visual>
</gazebo>
<gazebo reference="target/cone">
<visual>
<material>
<ambient> 1.0 1.0 1.0 0.0</ambient>
<diffuse> 1.0 1.0 1.0 0.0</diffuse>
<specular>1.0 1.0 1.0 0.0</specular>
</material>
</visual>
</gazebo> -->
<xacro:include filename="$(find rotors_description)/urdf/component_snippets.xacro" />
<xacro:default_imu namespace="target" parent_link="target/base_link" />
<gazebo>
<plugin name="landing_target_plugin" filename="librotors_gazebo_landing_target_plugin.so">
<robotNamespace>target</robotNamespace>
<linkName>target/base_link</linkName>
</plugin>
</gazebo>
<gazebo reference="target/base_link">
<mu1>0.00000001</mu1>
<mu2>0.00000001</mu2>
</gazebo>
</robot>
Here is the xacro file for the base of the drone:
<?xml version="1.0"?>
<!--
Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
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://ros.org/wiki/xacro">
<!-- Macro for the inertia of a box -->
<xacro:macro name="box_inertia" params="x y z mass">
<inertia ixx="${0.0833333 * mass * (y*y + z*z)}" ixy="0.0" ixz="0.0"
iyy="${0.0833333 * mass * (x*x + z*z)}" iyz="0.0"
izz="${0.0833333 * mass * (x*x + y*y)}" />
</xacro:macro>
<!-- Macro for the inertial properties of a box -->
<xacro:macro name="box_inertial" params="x y z mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
<xacro:box_inertia x="${x}" y="${y}" z="${z}" mass="${mass}" />
</inertial>
</xacro:macro>
<!-- Main multirotor link -->
<xacro:macro name="multirotor_base_macro"
params="robot_namespace mass body_width body_height use_mesh_file mesh_file *inertia">
<link name="${robot_namespace}/base_link">
</link>
<joint name="${robot_namespace}/base_joint" type="fixed">
<origin xyz="0 0 0" rpy=" 0 0 0" />
<parent link="${robot_namespace}/base_link" />
<child link="${robot_namespace}/base_link_inertia" />
</joint>
<link name="${robot_namespace}/base_link_inertia">
<inertial>
<mass value="${mass}" /> <!-- [kg] -->
<origin xyz="0 0 0" />
<xacro:insert_block name="inertia" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<xacro:if value="${use_mesh_file}">
<mesh filename="${mesh_file}" scale="1 1 1" />
</xacro:if>
<xacro:unless value="${use_mesh_file}">
<box size="${body_width} ${body_width} ${body_height}"/> <!-- [m] [m] [m] -->
</xacro:unless>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<xacro:if value="${use_mesh_file}">
<mesh filename="${mesh_file}" scale="1 1 1" />
</xacro:if>
<xacro:unless value="${use_mesh_file}">
<box size="${body_width} ${body_width} ${body_height}"/> <!-- [m] [m] [m] -->
</xacro:unless>
</geometry>
<contact_coefficients mu ="1" kp="1000" kd="1"/>
</collision>
</link>
<!-- attach multirotor_base_plugin to the base_link -->
<gazebo>
<plugin filename="librotors_gazebo_multirotor_base_plugin.so" name="multirotor_base_plugin">
<robotNamespace>${robot_namespace}</robotNamespace>
<linkName>${robot_namespace}/base_link</linkName>
<rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
</xacro:macro>
<!-- Rotor joint and link -->
<xacro:macro name="vertical_rotor"
params="robot_namespace suffix direction motor_constant moment_constant parent mass_rotor radius_rotor time_constant_up time_constant_down max_rot_velocity motor_number rotor_drag_coefficient rolling_moment_coefficient color use_own_mesh mesh *origin *inertia">
<joint name="${robot_namespace}/rotor_${motor_number}_joint" type="continuous">
<xacro:insert_block name="origin" />
<axis xyz="0 0 1" />
<!-- TODO(ff): not currently set because it's not yet supported -->
<!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
<parent link="${parent}" />
<child link="${robot_namespace}/rotor_${motor_number}" />
</joint>
<link name="${robot_namespace}/rotor_${motor_number}">
<inertial>
<mass value="${mass_rotor}" /> <!-- [kg] -->
<xacro:insert_block name="inertia" />
</inertial>
<visual>
<geometry>
<!-- <cylinder length="0.005" radius="${radius_rotor}"/> --> <!-- [m] -->
<xacro:if value="${use_own_mesh}">
<mesh filename="${mesh}"
scale="1 1 1" />
</xacro:if>
<xacro:unless value="${use_own_mesh}">
<mesh filename="package://rotors_description/meshes/propeller_${direction}.dae"
scale="${radius_rotor} ${radius_rotor} ${radius_rotor}" /> <!-- The propeller meshes have a radius of 1m -->
<!-- <box size="${2*radius_rotor} 0.01 0.005"/> -->
</xacro:unless>
</geometry>
</visual>
<collision>
<geometry>
<cylinder length="0.005" radius="${radius_rotor}" /> <!-- [m] -->
</geometry>
</collision>
</link>
<gazebo>
<plugin name="${robot_namespace}_${suffix}_motor_model" filename="librotors_gazebo_motor_model.so">
<robotNamespace>${robot_namespace}</robotNamespace>
<jointName>${robot_namespace}/rotor_${motor_number}_joint</jointName>
<linkName>${robot_namespace}/rotor_${motor_number}</linkName>
<turningDirection>${direction}</turningDirection>
<timeConstantUp>${time_constant_up}</timeConstantUp>
<timeConstantDown>${time_constant_down}</timeConstantDown>
<maxRotVelocity>${max_rot_velocity}</maxRotVelocity>
<motorConstant>${motor_constant}</motorConstant>
<momentConstant>${moment_constant}</momentConstant>
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
<motorNumber>${motor_number}</motorNumber>
<rotorDragCoefficient>${rotor_drag_coefficient}</rotorDragCoefficient>
<rollingMomentCoefficient>${rolling_moment_coefficient}</rollingMomentCoefficient>
<motorSpeedPubTopic>motor_speed/${motor_number}</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="${robot_namespace}/rotor_${motor_number}">
<material>Gazebo/${color}</material>
</gazebo>
</xacro:macro>
</robot>
I have faced a similar problem. The robot started to bounce when it was in contact with a surface. In my case, the source of problem was unrealistic inertia values of robot parts. Even if one of link has a bad calculated/estimated inertia, the model may bounce. I would propose you to verify calculations under inertial tags.

Multiple Realsense Cameras in URDF File

I have been working with ROS for only a couple weeks, sorry if i misunderstand things. I have made a URDF file with a robot in it. Now I wanted to add two Real-sense D435 cameras onto it so I can see it in Rviz. Following the test file given by Intel I got one camera to show up in the model however, when I try to create a second camera it just doesn't show up.
Question: How can I get two Real-sense cameras on my robot?
This is my URDF file:
<?xml version="1.0" ?>
<robot name="mrm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- BGN - Include -->
<xacro:include filename="$(find mrm_description)/urdf/robot_parameters.xacro" />
<xacro:include filename="$(find mrm_description)/urdf/links_joints.xacro" />
<xacro:include filename="$(find realsense2_camera)/urdf/_r435.urdf.xacro"/>
<xacro:include filename="$(find realsense2_camera)/urdf/_r430.urdf.xacro"/>
<m_link_box name="${link_00_name}"
origin_rpy="0 0 0" origin_xyz="0 0 0"
mass="1024"
ixx="170.667" ixy="0" ixz="0"
iyy="170.667" iyz="0"
izz="170.667"
size="1 1 1" />
<m_joint name="${link_00_name}__${link_01_name}" type="revolute"
axis_xyz="0 0 1"
origin_rpy="0 0 0" origin_xyz="0 0 0.5"
parent="base_link" child="link_01"
limit_e="1000" limit_l="-3.14" limit_u="3.14" limit_v="0.5" />
<m_link_mesh name="${link_01_name}"
origin_rpy="0 0 0" origin_xyz="0 0 -0.1"
mass="157.633"
ixx="13.235" ixy="0" ixz="0"
iyy="13.235" iyz="0"
izz="9.655"
meshfile="package://mrm_description/meshes/Link1-v2.stl"
meshscale="0.001 0.001 0.001" />
<m_joint name="${link_01_name}__${link_02_name}" type="revolute"
axis_xyz="0 1 0"
origin_rpy="0 0 0" origin_xyz="0 0 0.4"
parent="link_01" child="link_02"
limit_e="1000" limit_l="0" limit_u="0.5" limit_v="0.5" />
<m_link_cylinder name="${link_02_name}"
origin_rpy="0 0 0" origin_xyz="0 0 0.4"
mass="57.906"
ixx="12.679" ixy="0" ixz="0"
iyy="12.679" iyz="0"
izz="0.651"
radius="0.15" length="0.8" />
<m_joint name="${link_02_name}__${link_03_name}" type="revolute"
axis_xyz="0 1 0"
origin_rpy="0 0 0" origin_xyz="0 0 0.8"
parent="link_02" child="link_03"
limit_e="1000" limit_l="0" limit_u="0.75" limit_v="0.5" />
<m_link_cylinder name="${link_03_name}"
origin_rpy="0 0 0" origin_xyz="0 0 0.4"
mass="57.906"
ixx="12.679" ixy="0" ixz="0"
iyy="12.679" iyz="0"
izz="0.651"
radius="0.15" length="0.8" />
<m_joint name="${link_03_name}__${link_04_name}" type="revolute"
axis_xyz="0 1 0"
origin_rpy="0 0 0" origin_xyz="0 0 0.8"
parent="link_03" child="link_04"
limit_e="1000" limit_l="0" limit_u="0.75" limit_v="0.5" />
<m_link_cylinder name="${link_04_name}"
origin_rpy="0 0 0" origin_xyz="0 0 0.4"
mass="57.906"
ixx="12.679" ixy="0" ixz="0"
iyy="12.679" iyz="0"
izz="0.651"
radius="0.15" length="0.8" />
<m_joint name="${link_04_name}__${link_05_name}" type="revolute"
axis_xyz="0 0 1"
origin_rpy="0 0 0" origin_xyz="0 0 0.8"
parent="link_04" child="link_05"
limit_e="1000" limit_l="-3.14" limit_u="3.14" limit_v="0.5" />
<m_link_cylinder name="${link_05_name}"
origin_rpy="0 0 0" origin_xyz="0 0 0.125"
mass="18.056"
ixx="0.479" ixy="0" ixz="0"
iyy="0.479" iyz="0"
izz="0.204"
radius="0.15" length="0.25" />
<sensor_r435 name="Camera 1" parent="${link_05_name}">
<origin xyz="0 0 0" rpy="1 0 0"/>
</sensor_r435>
<sensor_r435 name="Camera 2" parent="${link_05_name}">
<origin xyz="0.17 0 0" rpy="0 0 0"/>
</sensor_r435>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
</plugin>
</gazebo>
</robot>
Looks like the realsense xacro doesn't have a "name" parameter which you are supplying, so it doesn't seem to do anything. Ideally, you can modify the intel xacro line -
<xacro:macro name="sensor_d415" params="parent *origin">
to
<xacro:macro name="sensor_d415" params="sensor_name parent *origin">
then add ${sensor_name}to every link and frame name in the intel xacro. Thus when you include this xacro in your URDF with different sensor_name, you recreate the full description each time.
If you get this working cleanly, please do submit a pull request to the intel github page.
In the meantime while waiting for a response I made a second file and added 2 to the end of each variable and that worked. Of course I knew this was not a permanent solution and that I should just keep it to one file. Thanks to Vik I have a solution. I took all the variables from my second file and replaced them with ${sensor_name}. For some reason the camera doesn't show up in rviz anymore. I'm not given any error messages but they just disappeared. Are there any issues with my code that people can see?
<?xml version="1.0"?>
<!--
License: Apache 2.0. See LICENSE file in root directory.
Copyright(c) 2017 Intel Corporation. All Rights Reserved
This is the URDF model for the Intel RealSense 430 camera, in it's
aluminum peripherial evaluation case.
-->
<robot name="sensor_d4352" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="sensor_d415" params="sensor_name parent *origin">
<xacro:property name="M_PI" value="3.1415926535897931" />
<!-- The following values are approximate, and the camera node
publishing TF values with actual calibrated camera extrinsic values -->
<xacro:property name="d435_cam_depth_to_left_ir_offset" value="0.0"/>
<xacro:property name="d435_cam_depth_to_right_ir_offset" value="-0.050"/>
<xacro:property name="d435_cam_depth_to_color_offset" value="0.015"/>
<!-- The following values model the aluminum peripherial case for the
D435 camera, with the camera joint represented by the actual
peripherial camera tripod mount -->
<xacro:property name="d435_cam_width" value="0.090"/>
<xacro:property name="d435_cam_height" value="0.025"/>
<xacro:property name="d435_cam_depth" value="0.02505"/>
<xacro:property name="d435_cam_mount_from_center_offset" value="0.0149"/>
<!-- The following offset is relative the the physical D435 camera peripherial
camera tripod mount -->
<xacro:property name="d435_cam_depth_px" value="${d435_cam_mount_from_center_offset}"/>
<xacro:property name="d435_cam_depth_py" value="0.0175"/>
<xacro:property name="d435_cam_depth_pz" value="${d435_cam_height/2}"/>
<material name="aluminum${sensor_name}">
<color rgba="0.5 0.5 0.5 1"/>
</material>
<!-- camera body, with origin at bottom screw mount -->
<joint name="camera_joint${sensor_name}" type="fixed">
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
<child link="camera_bottom_screw_frame${sensor_name}" />
</joint>
<link name="camera_bottom_screw_frame${sensor_name}"/>
<joint name="camera_link_joint${sensor_name}" type="fixed">
<origin xyz="0 ${d435_cam_depth_py} ${d435_cam_depth_pz}" rpy="0 0 0"/>
<parent link="camera_bottom_screw_frame${sensor_name}"/>
<child link="camera_link${sensor_name}" />
</joint>
<link name="camera_link${sensor_name}">
<visual>
<origin xyz="${d435_cam_mount_from_center_offset} ${-d435_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
<geometry>
<!-- <box size="${d435_cam_width} ${d435_cam_height} ${d435_cam_depth}"/> -->
<mesh filename="package://realsense2_camera/meshes/d435.dae" />
<!--<mesh filename="package://realsense2_camera/meshes/d435/d435.dae" />-->
</geometry>
<material name="aluminum"/>
</visual>
<collision>
<origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
<geometry>
<box size="${d435_cam_depth} ${d435_cam_width} ${d435_cam_height}"/>
</geometry>
</collision>
<inertial>
<!-- The following are not reliable values, and should not be used for modeling -->
<mass value="0.564" />
<origin xyz="0 0 0" />
<inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257" />
</inertial>
</link>
<!-- camera depth joints and links -->
<joint name="camera_depth_joint${sensor_name}" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="camera_link${sensor_name}"/>
<child link="camera_depth_frame${sensor_name}" />
</joint>
<link name="camera_depth_frame${sensor_name}"/>
<joint name="camera_depth_optical_joint${sensor_name}" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_depth_frame${sensor_name}" />
<child link="camera_depth_optical_frame${sensor_name}" />
</joint>
<link name="camera_depth_optical_frame${sensor_name}"/>
<!-- camera left IR joints and links -->
<joint name="camera_left_ir_joint${sensor_name}" type="fixed">
<origin xyz="0 ${d435_cam_depth_to_left_ir_offset} 0" rpy="0 0 0" />
<parent link="camera_depth_frame${sensor_name}" />
<child link="camera_left_ir_frame${sensor_name}" />
</joint>
<link name="camera_left_ir_frame${sensor_name}"/>
<joint name="camera_left_ir_optical_joint${sensor_name}" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_left_ir_frame${sensor_name}" />
<child link="camera_left_ir_optical_frame${sensor_name}" />
</joint>
<link name="camera_left_ir_optical_frame${sensor_name}"/>
<!-- camera right IR joints and links -->
<joint name="camera_right_ir_joint${sensor_name}" type="fixed">
<origin xyz="0 ${d435_cam_depth_to_right_ir_offset} 0" rpy="0 0 0" />
<parent link="camera_depth_frame${sensor_name}" />
<child link="camera_right_ir_frame${sensor_name}" />
</joint>
<link name="camera_right_ir_frame${sensor_name}"/>
<joint name="camera_right_ir_optical_joint${sensor_name}" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_right_ir_frame${sensor_name}" />
<child link="camera_right_ir_optical_frame${sensor_name}" />
</joint>
<link name="camera_right_ir_optical_frame${sensor_name}"/>
<!-- camera color joints and links -->
<joint name="camera_color_joint${sensor_name}" type="fixed">
<origin xyz="0 ${d435_cam_depth_to_color_offset} 0" rpy="0 0 0" />
<parent link="camera_depth_frame" />
<child link="camera_color_frame${sensor_name}" />
</joint>
<link name="camera_color_frame${sensor_name}"/>
<joint name="camera_color_optical_joint${sensor_name}" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_color_frame${sensor_name}" />
<child link="camera_color_optical_frame${sensor_name}" />
</joint>
<link name="camera_color_optical_frame"/>
</xacro:macro>
</robot>

Resources