@@ -18,10 +18,11 @@ cmake_minimum_required(VERSION 3.5)
 
 
 # Set proyect name
-project(micro_ros_agent)
+project(uros_agent)
 
 
 # Find packages depencences
+find_package(rosidl_cmake REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(fastcdr REQUIRED CONFIG)
 find_package(fastrtps REQUIRED CONFIG)
@@ -35,26 +36,178 @@ ament_export_dependencies(fastrtps)
 ament_export_dependencies(micrortps_agent)
 
 
-# Install python files
-ament_python_install_package(${PROJECT_NAME})
+# Set variables
+set(_OUTPUT_PATH "${CMAKE_CURRENT_BINARY_DIR}/python")
 
+set(_XML_INTERFACE_GEN_BIN "${_OUTPUT_PATH}/bin/Xml_interface_gen.py")
+normalize_path(_XML_INTERFACE_GEN_BIN "${_XML_INTERFACE_GEN_BIN}")
 
-# Register package resource in order to be called when a new msg package is generated
-ament_index_register_resource("rosidl_typesupport_c")
+set(_XML_DEFAULT_READ_BIN "${_OUTPUT_PATH}/bin/Xml_read_default_profiles.py")
+normalize_path(_XML_DEFAULT_READ_BIN "${_XML_DEFAULT_READ_BIN}")
 
+set(_PYTHON_PKG_TOOL ${PROJECT_NAME})
 
-# Install the package.xml file, and generate code for ``find_package`` so that other packages can get information about this package.
-ament_package(
-  CONFIG_EXTRAS "micro_ros_agent-extras.cmake.in"
+set(_RESOURCE_DIR "${_OUTPUT_PATH}/resource")
+normalize_path(_RESOURCE_DIR "${_RESOURCE_DIR}")
+
+
+set(_DEFAULT_FASTRTPS_PROFILES_PATH "${_OUTPUT_PATH}/gen/DEFAULT_FASTRTPS_PROFILES.xml")
+normalize_path(_DEFAULT_FASTRTPS_PROFILES_PATH "${_DEFAULT_FASTRTPS_PROFILES_PATH}")
+
+
+# Get colcon call dir
+get_filename_component(_COLCON_CALL_DIR "${CMAKE_INSTALL_PREFIX}" DIRECTORY)
+get_filename_component(_COLCON_CALL_DIR "${_COLCON_CALL_DIR}" DIRECTORY)
+
+
+# Generate python header
+set(
+_PYTHON_SCRIPT_HEAD
+"
+import os\n
+import sys\n
+sys.path.append('${_OUTPUT_PATH}')\n
+from ${_PYTHON_PKG_TOOL} import *\n
+"
 )
 
 
-install(
-  PROGRAMS bin/micro_ros_agent
-  DESTINATION lib/micro_ros_agent
+# Copy pyton files  
+file(COPY "bin" "${_PYTHON_PKG_TOOL}" "resource" DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/python")
+
+
+# Extract all packages and manifiest paths
+execute_process(
+  COMMAND
+  "${PYTHON_EXECUTABLE}"
+  "-c"
+  "${_PYTHON_SCRIPT_HEAD}for package in GetInterfacePackages(GetPackageList('${_COLCON_CALL_DIR}')): print ('%s,%s' % (package, GetPackageName(package)))"
+  OUTPUT_VARIABLE _packages
+  RESULT_VARIABLE _result
+  OUTPUT_STRIP_TRAILING_WHITESPACE
 )
+if(NOT _result EQUAL 0)
+  message(FATAL_ERROR "Error finding xml packages")
+endif()
+
+
+# Convert output to list
+string(REPLACE "\n" ";" _packages ${_packages})
+
+
+# Extract all msg from each package (stored in ${package}_MSG_FILES)
+set(ALL_MSG_FILES "")
+foreach(package ${_packages})
+  
+  # Extract info
+  string(REPLACE "," ";" package ${package})
+  list(GET package 0 package_file)
+  list(GET package 1 package)
+
+
+  # Get all msg files
+  execute_process(
+  COMMAND
+  "${PYTHON_EXECUTABLE}"
+  "-c"
+  "${_PYTHON_SCRIPT_HEAD}for msg in GetInterfacePackageMsgs('${package_file}'): print ('%s' % msg)"
+  OUTPUT_VARIABLE ${package}_MSG_FILES
+  RESULT_VARIABLE _result
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+  )
+  if(NOT _result EQUAL 0)
+    message(FATAL_ERROR "Error finding .msg files")
+  endif()
+
+  # Skip if there are no msgs to create
+  if(${package}_MSG_FILES STREQUAL "")
+    continue()
+  endif()
+
+  # Generate list
+  string(REPLACE "\n" ";" ${package}_MSG_FILES ${${package}_MSG_FILES})
+
+endforeach()
+
+
+# Append defaul xml profiles
+execute_process(
+  COMMAND "${PYTHON_EXECUTABLE}" "${_XML_DEFAULT_READ_BIN}" "--default-xml-path" "${_RESOURCE_DIR}"
+  OUTPUT_VARIABLE _XmlDoc
+  RESULT_VARIABLE _result
+  )
+if(NOT _result EQUAL 0)
+  message(FATAL_ERROR "Error in typesuppor generation") 
+endif()
+
 
+# Create one xml for each message
+foreach(package ${_packages})
+  string(REPLACE "," ";" package ${package})
+  list(GET package 0 package_file)
+  list(GET package 1 package)
+
+
+  # Skip this generation if there are no msgs to process
+  if(${package}_MSG_FILES STREQUAL "")
+    continue()
+  endif()
+
+
+
+  # generate script argument file 
+  set(generator_arguments_file "${_OUTPUT_PATH}/XML_ArgFiles/${package}_Args.json")
+  rosidl_write_generator_arguments(
+    "${generator_arguments_file}"
+    PACKAGE_NAME "${package}"
+    ROS_INTERFACE_FILES "${${package}_MSG_FILES}"
+    ROS_INTERFACE_DEPENDENCIES "NULL"
+    OUTPUT_DIR "NULL"
+    TEMPLATE_DIR "NULL"
+    TARGET_DEPENDENCIES "NULL"
+    ADDITIONAL_FILES ""
+  )
+
+
+   # execute python script
+   execute_process(
+    COMMAND "${PYTHON_EXECUTABLE}" "${_XML_INTERFACE_GEN_BIN}" --generator-arguments-file "${generator_arguments_file}"
+    OUTPUT_VARIABLE _XmlGen
+    RESULT_VARIABLE _result
+    )
+  if(NOT _result EQUAL 0)
+    message(FATAL_ERROR "Error in typesuppor generation") 
+  endif()
+
+
+  # Strore xml
+  set(_XmlDoc "${_XmlDoc}${_XmlGen}")
+  
+endforeach()
+
+
+# Close profile 
+set (_XmlDoc "<profiles>\n${_XmlDoc}</profiles>\n")
+
+
+# Save 
+file(WRITE "${_DEFAULT_FASTRTPS_PROFILES_PATH}" "${_XmlDoc}")
+
+
+
+# Install the package.xml file, and generate code for ``find_package`` so that other packages can get information about this package.
+ament_package()
+
+
+#Install
 install(
-  DIRECTORY cmake resource
-  DESTINATION share/${PROJECT_NAME}
-)
+  FILES "${_DEFAULT_FASTRTPS_PROFILES_PATH}"
+  DESTINATION "../micrortps_agent/bin"
+)
+
+
+# Install package
+#install(
+#  DIRECTORY cmake resource
+#  DESTINATION share/${PROJECT_NAME}
+#)
@@ -0,0 +1,30 @@
+#!/usr/bin/env python3
+
+import argparse
+import os
+import sys
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)),'..'))
+
+from rosidl_cmake import read_generator_arguments
+from rosidl_parser import UnknownMessageType
+from uros_agent import generate_XML
+
+
+def main(argv=sys.argv[1:]):
+    parser = argparse.ArgumentParser(
+        description='Generate the C interfaces for Micro RTPS.',
+        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument(
+        '--generator-arguments-file',
+        required=True,
+        help='The location of the file containing the generator arguments')
+    args = parser.parse_args(argv)
+
+    generator_args = read_generator_arguments(args.generator_arguments_file)
+    rc = generate_XML(generator_args)
+    return rc
+
+
+if __name__ == '__main__':
+    sys.exit(main())
@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import argparse
+import xml.etree.ElementTree
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)),'..'))
+
+from uros_agent import *
+
+def main(argv=sys.argv[1:]):
+    parser = argparse.ArgumentParser(
+        description='Import default xml files',
+        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument(
+        '--default-xml-path',
+        required=True,
+        help='The location of the default profiles')
+    args = parser.parse_args(argv)
+    ReadDefaultXMLs(args.default_xml_path)
+
+    return 0
+
+if __name__ == '__main__':
+    sys.exit(main())