Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add thermal camera to Marble HD2 #773

Merged
merged 8 commits into from
Feb 11, 2021
Merged
Original file line number Diff line number Diff line change
@@ -11,8 +11,9 @@
<arg name="top_scan" default="0"/>
<arg name="bottom_scan" default="0"/>
<arg name="lidar_3d" default="1"/>
<arg name="thermal" default="0"/>
<group ns="$(arg name)">
<!--Create multipe bridges so that it can run in parallel-->
<!--Create multiple bridges so that it can run in parallel-->
<node
pkg="ros_ign_bridge"
type="parameter_bridge"
@@ -71,6 +72,35 @@
<remap from="/model/$(arg name)/gas_detected" to="gas_detected"/>
</node>

<group if="$(arg thermal)">
<!-- Thermal Camera -->
<!-- Note camera_info topic generated by ign-gazebo incorrectly removes
sensor name in topic string -->
<node
pkg="ros_ign_bridge"
type="parameter_bridge"
name="ros_ign_bridge_thermal_camera"
args="$(arg sensor_prefix)/camera_info@sensor_msgs/CameraInfo[ignition.msgs.CameraInfo">
<remap from="$(arg sensor_prefix)/camera_info" to="thermal_camera/camera_info"/>
</node>
<node
pkg="ros_ign_image"
type="image_bridge"
name="ros_ign_image_thermal_camera"
args="$(arg sensor_prefix)/thermal_camera">
<remap from="$(arg sensor_prefix)/thermal_camera" to="thermal_camera/image_raw"/>
</node>
<node
pkg="subt_ros"
type="optical_frame_publisher"
name="optical_frame_publisher_image_thermal_camera">
<remap from="input/image" to="thermal_camera/image_raw" />
<remap from="output/image" to="thermal_camera/optical/image_raw" />
<remap from="input/camera_info" to="thermal_camera/camera_info" />
<remap from="output/camera_info" to="thermal_camera/optical/camera_info" />
</node>
</group>

<!-- stereo -->
<group if="$(arg stereo_cam)">
<node
8 changes: 8 additions & 0 deletions submitted_models/cerberus_m100_sensor_config_2/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
cmake_minimum_required(VERSION 2.8.3)
project(cerberus_m100_sensor_config_2)

find_package(catkin REQUIRED)

catkin_package()

install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
214 changes: 214 additions & 0 deletions submitted_models/cerberus_m100_sensor_config_2/launch/spawner.rb
Original file line number Diff line number Diff line change
@@ -0,0 +1,214 @@
def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
<<-HEREDOC
_config=8
uav=1
laserScan=0
stereoCam=0
rgbdCam=0
if _config == "2"
rgbdCam=1
end
if _config == "3" or _config == "4"
laserScan=1
end
if _config == "5"
stereoCam=1
if _config == "6"
laserScan=1
topScan=1
bottomScan=1
rgbdCam=1
end
if _config == "8"
laserScan=1
topScan=1
bottomScan=1
rgbdCam=1
end
<spawn name="${_name}">
<name>#{_name}</name>
<allow_renaming>false</allow_renaming>
<pose>#{_x} #{_y} #{_z + 0.187499} #{_roll} #{_pitch} #{_yaw}</pose>
<world>#{_worldName}</world>
<is_performer>true</is_performer>
<sdf version='1.6'>
<include>
<name>#{_name}</name>
<uri>#{_modelURI}</uri>
<!-- Publish robot state information -->
<plugin filename="libignition-gazebo-pose-publisher-system.so"
name="ignition::gazebo::systems::PosePublisher">
<publish_link_pose>true</publish_link_pose>
<publish_sensor_pose>true</publish_sensor_pose>
<publish_collision_pose>false</publish_collision_pose>
<publish_visual_pose>false</publish_visual_pose>
<publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
<use_pose_vector_msg>true</use_pose_vector_msg>
<static_publisher>true</static_publisher>
<static_update_frequency>1</static_update_frequency>
</plugin>
<plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>model/#{_name}</robotNamespace>
<jointName>rotor_0_joint</jointName>
<linkName>rotor_0</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>0.02246</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>0</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>0.5</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>model/#{_name}</robotNamespace>
<jointName>rotor_1_joint</jointName>
<linkName>rotor_1</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>0.02246</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>1</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>0.5</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>model/#{_name}</robotNamespace>
<jointName>rotor_2_joint</jointName>
<linkName>rotor_2</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>0.02246</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>2</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>0.5</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>model/#{_name}</robotNamespace>
<jointName>rotor_3_joint</jointName>
<linkName>rotor_3</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>0.02246</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>3</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>0.5</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin
filename="libignition-gazebo-multicopter-control-system.so"
name="ignition::gazebo::systems::MulticopterVelocityControl">
<robotNamespace>model/#{_name}</robotNamespace>
<commandSubTopic>cmd_vel</commandSubTopic>
<motorControlPubTopic>command/motor_speed</motorControlPubTopic>
<enableSubTopic>velocity_controller/enable</enableSubTopic>
<comLinkName>base_link</comLinkName>
<velocityGain>6 6 10</velocityGain>
<attitudeGain>4 4 2</attitudeGain>
<angularRateGain>0.7 0.7 0.7</angularRateGain>
<maximumLinearAcceleration>4 4 5</maximumLinearAcceleration>
<maximumLinearVelocity>3 3 1.8</maximumLinearVelocity>
<maximumAngularVelocity>3 3 1.9</maximumAngularVelocity>
<linearVelocityNoiseMean>0 0 0</linearVelocityNoiseMean>
<!-- linearVelocityNoiseStdDev based on error values reported in the paper Shen et. al., -->
<!-- Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor -->
<!-- http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.490.7958&rep=rep1&type=pdf -->
<linearVelocityNoiseStdDev>0.1105 0.1261 0.0947</linearVelocityNoiseStdDev>
<angularVelocityNoiseMean>0 0 0</angularVelocityNoiseMean>
<!-- angularVelocityNoiseStdDev values based on ADIS16448's Rate Noise Density with a sample -->
<!-- time of 0.004 ms. -->
<angularVelocityNoiseStdDev>0.004 0.004 0.004</angularVelocityNoiseStdDev>

<rotorConfiguration>
<rotor>
<jointName>rotor_0_joint</jointName>
<forceConstant>0.02246</forceConstant>
<momentConstant>1.6754e-2</momentConstant>
<direction>1</direction>
</rotor>
<rotor>
<jointName>rotor_1_joint</jointName>
<forceConstant>0.02246</forceConstant>
<momentConstant>1.6754e-2</momentConstant>
<direction>1</direction>
</rotor>
<rotor>
<jointName>rotor_2_joint</jointName>
<forceConstant>0.02246</forceConstant>
<momentConstant>1.6754e-2</momentConstant>
<direction>-1</direction>
</rotor>
<rotor>
<jointName>rotor_3_joint</jointName>
<forceConstant>0.02246</forceConstant>
<momentConstant>1.6754e-2</momentConstant>
<direction>-1</direction>
</rotor>
</rotorConfiguration>
</plugin>
<!-- Battery plugin -->
<plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
name="ignition::gazebo::systems::LinearBatteryPlugin">
<battery_name>linear_battery</battery_name>
<voltage>22.2</voltage>
<open_circuit_voltage_constant_coef>22.2</open_circuit_voltage_constant_coef>
<open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
<initial_charge>45.0</initial_charge>
<capacity>45.0</capacity>
<resistance>0.061523</resistance>
<smooth_current_tau>1.9499</smooth_current_tau>
<power_load>70</power_load>
<start_on_motion>true</start_on_motion>
</plugin>
<!-- Gas Detector plugin -->
<plugin filename="libGasEmitterDetectorPlugin.so"
name="subt::GasDetector">
<topic>/model/#{_name}/gas_detected</topic>
<update_rate>10</update_rate>
<type>gas</type>
</plugin>
</include>
</sdf>
</spawn>

HEREDOC
end

def rosExecutables(_name, _worldName)
<<-HEREDOC
<executable name='m100_description'>
<command>roslaunch --wait cerberus_m100_sensor_config_1 description.launch world_name:=#{_worldName} name:=#{_name}</command>
</executable>
<executable name='m100_ros_ign_bridge'>
<command>roslaunch --wait cerberus_m100_sensor_config_1 vehicle_topics.launch world_name:=#{_worldName} name:=#{_name} uav:=1 lidar_3d:=1 laser_scan:=0 top_scan:=0 bottom_scan:=0 stereo_cam:=0 thermal:=1</command>
</executable>
HEREDOC
end
23 changes: 23 additions & 0 deletions submitted_models/cerberus_m100_sensor_config_2/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<package format="2">
<name>cerberus_m100_sensor_config_2</name>
<version>0.1.0</version>
<description>
The cerberus_m100_sensor_config_2 package is derived from OSRF's X4 vehicle model.
This sensor configuration has a 3D lidar (copied from the X1 3D medium range lidar)
and a thermal camera
</description>

<author email="[email protected]">Mihir Kulkarni</author>
<author email="[email protected]">Paolo De Petris</author>
<maintainer email="[email protected]">Mihir Kulkarni</maintainer>
<maintainer email="[email protected]">Paolo De Petris</maintainer>

<license>BSD</license>

<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>roslaunch</buildtool_depend>
<buildtool_depend>urdf</buildtool_depend>
<buildtool_depend>xacro</buildtool_depend>

</package>
Original file line number Diff line number Diff line change
@@ -7,6 +7,7 @@
<arg name="sensor_prefix" value="/world/$(arg world_name)/model/$(arg name)/link/$(arg link_name)/sensor"/>
<arg name="gimbal_sensor_prefix" value="/world/$(arg world_name)/model/$(arg name)/link/tilt_gimbal_link/sensor"/>
<arg name="breadcrumbs" default="0"/>
<arg name="thermal" default="0"/>
<group ns="$(arg name)">
<!--Create multipe bridges so that it can run in parallel-->
<node
@@ -256,6 +257,34 @@
<remap from="/model/$(arg name)/gas_detected" to="gas_detected"/>
</node>

<group if="$(arg thermal)">
<!-- Note camera_info topic generated by ign-gazebo incorrectly removes
sensor name in topic string -->
<node
pkg="ros_ign_bridge"
type="parameter_bridge"
name="ros_ign_bridge_thermal_camera"
args="$(arg sensor_prefix)/camera_info@sensor_msgs/CameraInfo[ignition.msgs.CameraInfo">
<remap from="$(arg sensor_prefix)/camera_info" to="thermal_camera/camera_info"/>
</node>
<node
pkg="ros_ign_image"
type="image_bridge"
name="ros_ign_image_thermal_camera"
args="$(arg sensor_prefix)/thermal_camera">
<remap from="$(arg sensor_prefix)/thermal_camera" to="thermal_camera/image_raw"/>
</node>
<node
pkg="subt_ros"
type="optical_frame_publisher"
name="optical_frame_publisher_image_thermal_camera">
<remap from="input/image" to="thermal_camera/image_raw" />
<remap from="output/image" to="thermal_camera/optical/image_raw" />
<remap from="input/camera_info" to="thermal_camera/camera_info" />
<remap from="output/camera_info" to="thermal_camera/optical/camera_info" />
</node>
</group>

<group if="$(arg breadcrumbs)">
<node
pkg="ros_ign_bridge"
Loading