Skip to content

Commit

Permalink
Separate emulator launch and connection configs
Browse files Browse the repository at this point in the history
  • Loading branch information
@song-ms @doosan-robotics
song-ms authored and doosan-robotics committed Jan 21, 2025
1 parent 3d9ca0b commit e2426bd
Show file tree
Hide file tree
Showing 17 changed files with 579 additions and 126 deletions.
75 changes: 32 additions & 43 deletions dsr_bringup2/dsr_bringup2/connection.py → dsr_bringup2/dsr_bringup2/run_emulator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,38 +6,9 @@
import rclpy
from rclpy.node import Node

class VirtualDRCF():
def __init__(self, port, model, name):
self.emulator_name = "emulator"
if name:
self.emulator_name = name + "_" + "emulator"
self.run_drcf(port, model, name)
# signal.signal(signal.SIGINT, self.terminate_drcf)
# signal.signal(signal.SIGTERM, self.terminate_drcf)

## TODO(Improve) : The context seems to can't handle signal perfectly.
# needed to fix after https://github.com/ros2/rclpy/issues/1287
def __del__(self):
self.terminate_drcf()

def run_drcf(self, port, model, name):
run_script_path = os.path.join(
get_package_share_directory("common2"), "bin"
)
start_cmd = "{}/run_drcf.sh ".format(run_script_path) +" "+ str(port)+" "+ model +" " +name
os.system(start_cmd)

def terminate_drcf(self):
stop_cmd = "docker ps -a --filter name={} -q | xargs -r docker stop".\
format(self.emulator_name)
print("stop_cmd : ",stop_cmd)
os.system(stop_cmd)
rclpy.shutdown()
exit(1)

class ConnectionNode(Node):
class VirtualDRCF(Node):
def __init__(self):
super().__init__('connection_node')
super().__init__('virtual_node')

# 파라미터 선언
self.declare_parameter('name', 'dsr01')
Expand Down Expand Up @@ -65,23 +36,41 @@ def __init__(self):
parameters['mobile'] = self.get_parameter('mobile').get_parameter_value().string_value
parameters['rt_host'] = self.get_parameter('rt_host').get_parameter_value().string_value

current_file_path = os.path.join(
get_package_share_directory("dsr_hardware2"), "config"


port, model, name = parameters['port'], parameters['model'], parameters['name']
self.emulator_name = "emulator"
if name:
self.emulator_name = name + "_" + "emulator"
self.run_drcf(port, model, name)

# signal.signal(signal.SIGINT, self.terminate_drcf)
# signal.signal(signal.SIGTERM, self.terminate_drcf)

## TODO(Improve) : The context seems to can't handle signal perfectly.
# needed to fix after https://github.com/ros2/rclpy/issues/1287
def __del__(self):
self.terminate_drcf()

def run_drcf(self, port, model, name):
run_script_path = os.path.join(
get_package_share_directory("common2"), "bin"
)
os.makedirs(current_file_path, exist_ok=True)
param_name = self.get_namespace()[1:] +'_parameters.yaml'
with open(os.path.join(current_file_path, param_name), 'w') as file:
yaml.dump(parameters, file)
start_cmd = "{}/run_drcf.sh ".format(run_script_path) +" "+ str(port)+" "+ model +" " +name
os.system(start_cmd)

def terminate_drcf(self):
stop_cmd = "docker ps -a --filter name={} -q | xargs -r docker stop".\
format(self.emulator_name)
print("stop_cmd : ",stop_cmd)
os.system(stop_cmd)
rclpy.shutdown()
exit(1)

## Run Virtual DRCF if necessary.
if parameters['mode'] == 'virtual':
port, model, name = parameters['port'], parameters['model'], parameters['name']
self.get_logger().info('@@@@@@@@@@@@@@@@@@@@@@@@@@@ run_drcf')
self.virtual_drcf = VirtualDRCF(port, model, name)

def main(args=None):
rclpy.init(args=args)
node = ConnectionNode()
node = VirtualDRCF()
rclpy.spin(node)
rclpy.shutdown()

Expand Down
55 changes: 55 additions & 0 deletions dsr_bringup2/dsr_bringup2/set_config.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
import os
import yaml
import signal

from ament_index_python.packages import get_package_share_directory
import rclpy
from rclpy.node import Node


class ConnectionNode(Node):
def __init__(self):
super().__init__('set_config_node')

# 파라미터 선언
self.declare_parameter('name', 'dsr01')
self.declare_parameter('rate', 100)
self.declare_parameter('standby', 5000)
self.declare_parameter('command', True)
self.declare_parameter('host', '127.0.0.1')
self.declare_parameter('port', 12345)
self.declare_parameter('mode', 'virtual')
self.declare_parameter('model', 'm1013')
self.declare_parameter('gripper', 'none')
self.declare_parameter('mobile', 'none')
self.declare_parameter('rt_host', '192.168.137.50')

parameters = {}
parameters['name'] = self.get_parameter('name').get_parameter_value().string_value
parameters['rate'] = self.get_parameter('rate').get_parameter_value().integer_value
parameters['standby'] = self.get_parameter('standby').get_parameter_value().integer_value
parameters['command'] = self.get_parameter('command').get_parameter_value().bool_value
parameters['host'] = self.get_parameter('host').get_parameter_value().string_value
parameters['port'] = self.get_parameter('port').get_parameter_value().integer_value
parameters['mode'] = self.get_parameter('mode').get_parameter_value().string_value
parameters['model'] = self.get_parameter('model').get_parameter_value().string_value
parameters['gripper'] = self.get_parameter('gripper').get_parameter_value().string_value
parameters['mobile'] = self.get_parameter('mobile').get_parameter_value().string_value
parameters['rt_host'] = self.get_parameter('rt_host').get_parameter_value().string_value

current_file_path = os.path.join(
get_package_share_directory("dsr_hardware2"), "config"
)
os.makedirs(current_file_path, exist_ok=True)
param_name = self.get_namespace()[1:] +'_parameters.yaml'
with open(os.path.join(current_file_path, param_name), 'w') as file:
yaml.dump(parameters, file)
os.system("sync")

def main(args=None):
rclpy.init(args=args)
ConnectionNode()
rclpy.shutdown()

if __name__ == '__main__':
main()
41 changes: 35 additions & 6 deletions dsr_bringup2/launch/dsr_bringup2_gazebo.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,9 +81,9 @@ def generate_launch_description():
[FindPackageShare("dsr_description2"), "rviz", "default.rviz"]
)

connection_node = Node(
set_config_node = Node(
package="dsr_bringup2",
executable="connection",
executable="set_config",
namespace=LaunchConfiguration('name'),
parameters=[
{"name": LaunchConfiguration('name') },
Expand All @@ -97,7 +97,28 @@ def generate_launch_description():
{"gripper": "none" },
{"mobile": "none" },
{"rt_host": LaunchConfiguration('rt_host') },
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
],
output="screen",
)

run_emulator_node = Node(
package="dsr_bringup2",
executable="run_emulator",
namespace=LaunchConfiguration('name'),
parameters=[
{"name": LaunchConfiguration('name') },
{"rate": 100 },
{"standby": 5000 },
{"command": True },
{"host": LaunchConfiguration('host') },
{"port": LaunchConfiguration('port') },
{"mode": LaunchConfiguration('mode') },
{"model": LaunchConfiguration('model') },
{"gripper": "none" },
{"mobile": "none" },
{"rt_host": LaunchConfiguration('rt_host') },
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
],
output="screen",
)
Expand Down Expand Up @@ -197,16 +218,24 @@ def generate_launch_description():
on_exit=[included_launch],
)
)

# Delay start of robot_controller after `joint_state_broadcaster`
delay_control_node_after_connection_node = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=set_config_node,
on_exit=[control_node],
)
)

nodes = [
connection_node,
run_emulator_node,
gazebo_connection_node,
control_node,
robot_state_pub_node,
robot_controller_spawner,
joint_state_broadcaster_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
included_launch_after_robot_controller_spawner
included_launch_after_robot_controller_spawner,
delay_control_node_after_connection_node,
]

return LaunchDescription(ARGUMENTS + nodes)
40 changes: 34 additions & 6 deletions dsr_bringup2/launch/dsr_bringup2_rviz.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,9 +74,30 @@ def generate_launch_description():
[FindPackageShare("dsr_description2"), "rviz", "default.rviz"]
)

connection_node = Node(
set_config_node = Node(
package="dsr_bringup2",
executable="connection",
executable="set_config",
namespace=LaunchConfiguration('name'),
parameters=[
{"name": LaunchConfiguration('name') },
{"rate": 100 },
{"standby": 5000 },
{"command": True },
{"host": LaunchConfiguration('host') },
{"port": LaunchConfiguration('port') },
{"mode": LaunchConfiguration('mode') },
{"model": LaunchConfiguration('model') },
{"gripper": "none" },
{"mobile": "none" },
{"rt_host": LaunchConfiguration('rt_host') },
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
],
output="screen",
)

run_emulator_node = Node(
package="dsr_bringup2",
executable="run_emulator",
namespace=LaunchConfiguration('name'),
parameters=[
{"name": LaunchConfiguration('name') },
Expand Down Expand Up @@ -165,15 +186,22 @@ def generate_launch_description():
on_exit=[robot_controller_spawner],
)
)


# Delay start of robot_controller after `joint_state_broadcaster`
delay_control_node_after_connection_node = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=set_config_node,
on_exit=[control_node],
)
)

nodes = [
connection_node,
control_node,
run_emulator_node,
robot_state_pub_node,
robot_controller_spawner,
joint_state_broadcaster_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
# delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
delay_control_node_after_connection_node,
]

return LaunchDescription(ARGUMENTS + nodes)
41 changes: 35 additions & 6 deletions dsr_bringup2/launch/dsr_bringup2_spawn_on_gazebo.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,9 +81,9 @@ def generate_launch_description():
[FindPackageShare("dsr_description2"), "rviz", "default.rviz"]
)

connection_node = Node(
set_config_node = Node(
package="dsr_bringup2",
executable="connection",
executable="set_config",
namespace=LaunchConfiguration('name'),
parameters=[
{"name": LaunchConfiguration('name') },
Expand All @@ -97,7 +97,28 @@ def generate_launch_description():
{"gripper": "none" },
{"mobile": "none" },
{"rt_host": LaunchConfiguration('rt_host') },
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
],
output="screen",
)

run_emulator_node = Node(
package="dsr_bringup2",
executable="run_emulator",
namespace=LaunchConfiguration('name'),
parameters=[
{"name": LaunchConfiguration('name') },
{"rate": 100 },
{"standby": 5000 },
{"command": True },
{"host": LaunchConfiguration('host') },
{"port": LaunchConfiguration('port') },
{"mode": LaunchConfiguration('mode') },
{"model": LaunchConfiguration('model') },
{"gripper": "none" },
{"mobile": "none" },
{"rt_host": LaunchConfiguration('rt_host') },
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
],
output="screen",
)
Expand Down Expand Up @@ -212,16 +233,24 @@ def generate_launch_description():
on_exit=[included_launch],
)
)

# Delay start of robot_controller after `joint_state_broadcaster`
delay_control_node_after_connection_node = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=set_config_node,
on_exit=[control_node],
)
)

nodes = [
connection_node,
run_emulator_node,
gazebo_connection_node,
control_node,
robot_state_pub_node,
robot_controller_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
included_launch_after_robot_controller_spawner
included_launch_after_robot_controller_spawner,
delay_control_node_after_connection_node,
]

return LaunchDescription(ARGUMENTS + nodes)
3 changes: 2 additions & 1 deletion dsr_bringup2/setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,10 @@
tests_require=['pytest'],
entry_points={
'console_scripts': [
'connection = dsr_bringup2.connection:main',
'set_config = dsr_bringup2.set_config:main',
'moveit_connection = dsr_bringup2.moveit_connection:main',
'gazebo_connection = dsr_bringup2.gazebo_connection:main',
'run_emulator = dsr_bringup2.run_emulator:main',
],
},
)
Loading

0 comments on commit e2426bd

Please sign in to comment.