add new env_manager to project

env_manager/env_manager/config/test_params.yaml

@@ -1,9 +0,0 @@
-env_manager:
-  ros__parameters:
-    gz_enviroment:
-      type: gz_enviroment/GzEnviroment
-  
-
-  gz_enviroment:
-    ros__parameters:
-      mesh_name: "monkey"

env_manager/env_manager/package.xml

@@ -9,10 +9,10 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   
+  <depend>env_interface</depend>
   <depend>env_manager_interfaces</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
-  <depend>env_inteface</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

env_manager/gz_enviroment/src/gz_enviroment.cpp

@@ -45,10 +45,10 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State&)
   m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
   m_service_spawn = std::string("/world/") + m_world_name + "/create";
 
-  if (!doGzSpawn())
-  {
-    return CallbackReturn::ERROR;
-  }
+  // if (!doGzSpawn())
+  // {
+  //   return CallbackReturn::ERROR;
+  // }
 
   return CallbackReturn::SUCCESS;
 }
@@ -85,7 +85,7 @@ CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&)
 void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
 {
   // TODO: Read from config
-  m_follow_frames = { "box1", "box2", "box3", "box4", "box5", "box6", "monkey" };
+  m_follow_frames = { "box1", "box2", "box3", "box4", "box5", "box6"};
   for (const auto& it : pose_v.pose())
   {
     if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end())

rbs_bringup/launch/bringup.launch.py

@@ -142,7 +142,7 @@ def generate_launch_description():
         DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
+        DeclareLaunchArgument("env_manager", default_value="true", description="Launch env_manager?")
     )
     declared_arguments.append(
         DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg")

rbs_bt_executor/CMakeLists.txt

@@ -18,7 +18,6 @@ find_package(ament_index_cpp REQUIRED)
 find_package(rbs_skill_interfaces REQUIRED)
 find_package(behavior_tree REQUIRED)
 find_package(control_msgs REQUIRED)
-find_package(component_interfaces REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 
@@ -40,7 +39,6 @@ set(dependencies
     behavior_tree
     std_msgs
     control_msgs
-    component_interfaces
     lifecycle_msgs
     rcl_interfaces
 )

rbs_simulation/launch/simulation.launch.py

@@ -63,9 +63,8 @@ def generate_launch_description():
                     '-topic', '/robot_description'],
                  output='screen',
                  condition=IfCondition(sim_gazebo))
-    
     env_manager = Node(package="env_manager", 
-                       executable="env_manager_node",
+                       executable="run_env_manager",
                        condition=IfCondition(env_manager_cond)
                        )