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.

Sign up for GitHub

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 node that fakes object detection information in sim #625

Closed
wants to merge 3 commits into from
Closed

Add node that fakes object detection information in sim #625

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
93aba99
add node to fake obj pose in sim
alisonryckman Dec 14, 2023
d5c590a
add mallet model can't figure out how to make it orange :(
alisonryckman Dec 18, 2023
4e1ddf5
parameterize object name and add node to sim launch file
alisonryckman Dec 19, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions config/gazebo/env_description/mallet/meshes/mallet.dae
View file
Open in desktop
Git LFS file not shown
131 changes: 131 additions & 0 deletions config/gazebo/env_description/world_flat_mallet.world
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic' />
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.7 0.7 0.7 1</ambient>
<background>0.529 0.808 0.922 1</background>
<shadows>1</shadows>
</scene>
<wind />
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='mallet'>
<pose>0 5 0 0 0 4.71239</pose>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://mallet/meshes/mallet.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode />
</contact>
<bounce />
<friction>
<torsional>
<ode />
</torsional>
<ode />
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://mallet/meshes/mallet.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='ground'>
<pose>0 0 -0.1 0 0 0</pose>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://ground.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<collide_bitmask>65535</collide_bitmask>
<ode />
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode />
</torsional>
</friction>
<bounce />
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://ground.dae</uri>
</mesh>
</geometry>
<material>
<shader type="normal_map_object_space">
<normal_map>model://soil_normal.png</normal_map>
</shader>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>6.01711 -3.68905 2.02916 0 0.275643 2.35619</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>
10 changes: 9 additions & 1 deletion launch/auton_sim.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
This launch file launches everything necessary to run auton code in the simulator.
-->
<launch>
<arg name="publish_object_location" default="false"/>
<arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz"/>
<arg name="world_name" default="world_flat"/>

Expand All @@ -15,6 +16,13 @@
<arg name="world_name" value="$(arg world_name)"/>
</include>

<node if="$(arg publish_object_location)"
pkg="mrover" type="publish_gazebo_object.py" name="publish_object" args="mallet">
<!-- $(arg world_name) should contain the object whose location you are publishing -->
<!-- e.g. world_flat_mallet -->
<param name="object_name" value="mallet"/>
</node>

<node pkg="tf" type="static_transform_publisher" name="zed2i_left_camera_frame_publisher"
args="0 0 0 0 0 0 1 left_camera_link zed2i_left_camera_frame 100"/>

Expand All @@ -24,4 +32,4 @@
<arg name="use_ekf" value="true"/>
<arg name="ekf_start_delay" value="5"/>
</include>
</launch>
</launch>
43 changes: 43 additions & 0 deletions src/simulator/publish_gazebo_object.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
#!/usr/bin/env python3

import rospy
import numpy as np
import tf2_ros
from gazebo_msgs.msg import ModelStates
from util.SE3 import SE3


class GazeboObject:
def __init__(self):
rospy.Subscriber("gazebo/model_states", ModelStates, self.state_callback)
self.tf_broadcaster = tf2_ros.TransformBroadcaster()
self.obj_to_detect = rospy.get_param("publish_object/object_name")
# To prevent tf_repeated_data warning
self.ts = rospy.Time.now()

def state_callback(self, msg: ModelStates):
try:
msg_time = rospy.Time.now()
if self.ts != msg_time:
self.ts = msg_time
obj_index = msg.name.index(self.obj_to_detect)
msg_pos = msg.pose[obj_index]
pos = np.array([msg_pos.position.x, msg_pos.position.y, msg_pos.position.z])
rot = np.array(
[msg_pos.orientation.x, msg_pos.orientation.y, msg_pos.orientation.z, msg_pos.orientation.w]
)
obj_pose = SE3.from_pos_quat(pos, rot)
obj_pose.publish_to_tf_tree(self.tf_broadcaster, "map", "object_link")
except ValueError:
rospy.logerr("object not in sim world")
return


def main():
rospy.init_node("publish_gazebo_object")
gazebo_object = GazeboObject()
rospy.spin()


if __name__ == "__main__":
main()