diff --git a/launch/sim_node_launch.py b/launch/sim_node_launch.py index 123e2e2..ed4a2dc 100644 --- a/launch/sim_node_launch.py +++ b/launch/sim_node_launch.py @@ -78,6 +78,14 @@ def generate_launch_description(): "use_sim_time", default_value="false", ), + DeclareLaunchArgument( + "primary_robot_color", + default_value="[0.0, 0.0, 1.0, 1.0]", + ), + DeclareLaunchArgument( + "secondary_robot_color", + default_value="[0.0, 0.0, 1.0, 1.0]", + ) ] node = Node( @@ -103,6 +111,8 @@ def generate_launch_description(): "lidar_pointcloud_resolution" ), "use_sim_time": LaunchConfiguration("use_sim_time"), + "primary_robot_color": LaunchConfiguration("primary_robot_color"), + "secondary_robot_color": LaunchConfiguration("secondary_robot_color"), } ], output="screen", diff --git a/sim_node/infantry_robot.py b/sim_node/infantry_robot.py index 7acaebc..639172f 100644 --- a/sim_node/infantry_robot.py +++ b/sim_node/infantry_robot.py @@ -37,10 +37,10 @@ def __init__( build_separate: bool = False, options: dict = [], ): + self.options = options super().__init__( scene, control_freq, control_mode, agent_idx, initial_pose, build_separate ) - self.options = options default_pos = sapien.Pose(p=[0, 0, 0.25], q=[1, 0, 0, 0]) default_pos.set_rpy([np.deg2rad(90), 0, np.deg2rad(45)]) @@ -80,8 +80,10 @@ def _after_loading_articulation(self): visual_block = lightbar_link.render_shapes[0] for shape in visual_block: for part in shape.parts: - part.material.set_base_color([0, 0, 1, 1]) - part.material.set_emission([0, 0, 100, 100]) + if "color" in self.options: + color = self.options["color"] + part.material.set_base_color(color) # Set rgba list of float + part.material.set_emission([x * 100 for x in color]) # Multiply by scalar part.material.set_emission_texture(None) part.material.set_metallic_texture(None) part.material.set_roughness_texture(None) diff --git a/sim_node/ros_bridge.py b/sim_node/ros_bridge.py index 14d2c27..d48fbbc 100644 --- a/sim_node/ros_bridge.py +++ b/sim_node/ros_bridge.py @@ -47,6 +47,9 @@ def __init__(self): self.declare_parameter("sim_time_scale", 1.0) self.declare_parameter("cpu_sim", False) + self.declare_parameter("primary_robot_color", [0.0, 0.0, 1.0, 1.0]) + self.declare_parameter("secondary_robot_color", [0.0, 0.0, 1.0, 1.0]) + self.primary_robot_teleop_sub = self.create_subscription( SimTeleopInput, "simulation/primary_robot_teleop", @@ -134,9 +137,10 @@ def __init__(self): ) .get_parameter_value() .integer_value, + color=self.get_parameter("primary_robot_color").get_parameter_value().double_array_value, ), # second robot is just used as target practice so we dont care about its sensors - secondary_robot=dict(enable_cv_cam=False, enable_lidar=False), + secondary_robot=dict(enable_cv_cam=False, enable_lidar=False, color=self.get_parameter("secondary_robot_color").get_parameter_value().double_array_value), ) self.simulation = simulation.Simulation(options=options)