Merge pull request #147 from facebookresearch/vslam_debug

Vslam Config fix

robots/LoCoBot/locobot_control/launch/vslam_debug.launch

@@ -0,0 +1,122 @@
+<launch>
+    <arg name="use_camera" default="true"/>
+    <arg name="use_arm" default="false"/>
+    <arg name="use_vslam" default="true"/>
+    <arg name="use_base" default="true"/>
+    <arg name="torque_control" default="false"/>
+    <arg name="use_sim" default="false"/>
+    <arg name="teleop" default="false"/>
+    <arg name="use_rviz" default="true"/>
+    <arg name="base" default="kobuki"/> <!-- Options: create and kobuki -->
+
+    <param name="use_camera" value="$(arg use_camera)"/>
+    <param name="use_arm" value="$(arg use_arm)"/>
+    <param name="use_vslam" value="$(arg use_vslam)"/>
+    <param name="use_sim" value="$(arg use_sim)"/>
+    <param name="use_base" value="$(arg use_base)"/>
+    <param name="torque_control" value="$(arg torque_control)"/>
+    <param name="teleop" value="$(arg teleop)"/>
+
+
+
+    <group unless="$(arg use_sim)">
+
+        <group if="$(arg use_camera)">
+            <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
+                <arg name="enable_pointcloud" value="false"/>
+            </include>
+        </group>
+
+        <group if="$(arg use_vslam)">
+            <group if="$(arg use_camera)">
+                  <include file="$(find orb_slam2_ros)/launch/orb_slam2_realsense.launch">
+                      <arg name="launch_realsense" value="false"/>
+                  </include>
+            </group>
+            <group unless="$(arg use_camera)">
+                  <include file="$(find orb_slam2_ros)/launch/orb_slam2_realsense.launch"/>
+            </group>
+        </group>
+
+        <group if="$(arg use_base)" >
+
+            <include file="$(find base_navigation)/launch/main_base.launch">
+                <arg name="base" value="$(arg base)"/>
+            </include>
+
+        </group>
+
+        <group if="$(arg teleop)">
+            <include file="$(find locobot_control)/launch/dynamixel_controllers.launch">
+                <arg name="dynamixel_info" value="$(find locobot_control)/config/dynamixels_teleop.yaml"/>
+            </include>
+        </group>
+        <group unless="$(arg teleop)">
+            <group if="$(eval arg('use_arm') or arg('use_camera') or arg('use_vslam'))">
+                <include file="$(find locobot_control)/launch/dynamixel_controllers.launch"/>
+            </group>
+        </group>
+
+        <node name="calibration_tf_broadcaster" pkg="locobot_calibration"
+              type="calibration_publish_transforms.py"/>
+
+    </group>
+
+    <!-- Gazebo -->
+    <group if="$(arg use_sim)">
+
+        <include file="$(find locobot_gazebo)/launch/gazebo_locobot.launch">
+            <arg name="base" value="$(arg base)"/>
+        </include>
+
+        <include file="$(find locobot_gazebo)/launch/gazebo_locobot_control.launch"/>
+        <node name="locobot_gazebo" pkg="locobot_gazebo" type="locobot_gazebo" 
+            respawn="true" output="screen"/> 
+
+        <group if="$(arg use_vslam)">
+
+          <include file="$(find orb_slam2_ros)/launch/orb_slam2_realsense.launch">
+              <arg name="launch_realsense" value="false"/>
+          </include>
+
+        </group>
+
+
+    </group>
+
+
+    <!-- Common -->
+
+    <node name="turtlebot_controller_node" pkg="turtlebot_controller" 
+                            type="turtlebot_controller_node"/>
+<!-- 
+
+    <node name="pyrobot_kinematics" pkg="pyrobot_bridge"
+                  type="kinematics.py"/>
+
+    <node name="pyrobot_moveit" pkg="pyrobot_bridge"
+                  type="moveit_bridge.py"/>
+
+ -->
+
+    <group if="$(eval arg('use_base') or arg('use_sim'))" >
+        <include file="$(find base_navigation)/launch/move_base.launch">
+            <arg name="use_map" value="$(arg use_vslam)"/>
+            <arg name="base" value="$(arg base)"/>
+        </include>
+    </group>
+
+
+    <group if="$(eval base =='create')">
+        <include file="$(find locobot_lite_moveit_config)/launch/demo.launch">
+            <arg name="use_rviz" value="$(arg use_rviz)"/>
+        </include>
+    </group>
+
+    <group if="$(eval base =='kobuki')">
+        <include file="$(find locobot_moveit_config)/launch/demo.launch">
+            <arg name="use_rviz" value="$(arg use_rviz)"/>
+        </include>
+    </group>
+
+</launch>

robots/LoCoBot/locobot_control/src/locobot_controller.cpp

@@ -73,7 +73,7 @@ LoCoBotController::~LoCoBotController() {
   if (use_group_["arm"]) {
     if (!torque_control_) {
       std::map<std::string, float> restJntsMap;
-      float restJnts[5] = {0, -0.3890, 1.617, -0.1812, 0.0153};
+      float restJnts[5] = {0, -1.30, 1.617, 0.5, 0};
       restJntsMap["joint_1"] = restJnts[0];
       restJntsMap["joint_2"] = restJnts[1];
       restJntsMap["joint_3"] = restJnts[2];

robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/global_costmap_params_with_map.yaml

@@ -1,6 +1,6 @@
 global_costmap:
-   global_frame: /map
-   robot_base_frame: /base_link
+   global_frame: map
+   robot_base_frame: base_link
    update_frequency: 1.0
    publish_frequency: 0.5
    static_map: true

robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/global_map_extra.yaml

@@ -1,5 +1,5 @@
 static_layer:
-  map_topic: "/occupancy_map"
+  map_topic: "occupancy_map"
   first_map_only: false
 
 inflation_layer:

robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/local_costmap_params.yaml

@@ -1,6 +1,6 @@
 local_costmap:
    global_frame: map
-   robot_base_frame: /base_link
+   robot_base_frame: base_link
    update_frequency: 5.0
    publish_frequency: 2.0
    static_map: false

robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/local_map_extra.yaml

@@ -1,5 +1,5 @@
 static_layer:
-  map_topic: "/occupancy_map"
+  map_topic: "occupancy_map"
   first_map_only: false
 
 inflation_layer:

robots/LoCoBot/locobot_navigation/base_navigation/launch/turtlebot_move_base.launch

@@ -25,9 +25,9 @@
       <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
       <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
       <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>
-      <rosparam file="$(find base_navigation)/config/turtlebot/global_map_extra.yaml"
+<!--       <rosparam file="$(find base_navigation)/config/turtlebot/global_map_extra.yaml"
                   command="load"
-                  ns="global_costmap" />
+                  ns="global_costmap" /> -->
       <rosparam file="$(find base_navigation)/config/turtlebot/local_map_extra.yaml"
                   command="load"
                   ns="local_costmap" />

robots/LoCoBot/locobot_navigation/base_navigation/nodes/mapToGrid.py

@@ -109,6 +109,7 @@ def initGrid(self):
     def updateGrid(self):
         if self.points is None:
             return
+        print(CELL_RESOLUTION, self.xCells, self.yCells, self.xMin, self.yMin, self.ocGrid)
         self.grid.header.seq = self.grid.header.seq + 1
         self.grid.header.stamp = rospy.Time.now()
         self.grid.info.map_load_time = rospy.Time.now()

robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/pcdlib.py

@@ -233,7 +233,7 @@ def get_current_pcd(self):
             return None, None
         num_frames = 0
         num_pts = 0
-        for KFid, ext_mat in self.extrinsic_mats.iteritems():
+        for KFid, ext_mat in self.extrinsic_mats.items():
             if KFid not in self.pcd_pool_in_cam:
                 continue
             num_frames += 1
@@ -244,7 +244,7 @@ def get_current_pcd(self):
         all_pts = np.zeros((num_pts, 3))
         all_colors = np.zeros((num_pts, 3))
         cur_id = 0
-        for KFid, ext_mat in self.extrinsic_mats.iteritems():
+        for KFid, ext_mat in self.extrinsic_mats.items():
             if KFid not in self.pcd_pool_in_cam:
                 continue
             pcd_in_cam, rgb = self.pcd_pool_in_cam[KFid]

robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/vslam.py

@@ -253,7 +253,7 @@ def get_occupancy_map(self):
             xcells = (int((self.x_max - self.x_min) / self.map_resultion)) + 1
             ycells = (int((self.y_max - self.y_min) / self.map_resultion)) + 1
             occ_map = np.zeros(xcells * ycells)
-            return occ_map, xcells, ycells, self.x_min, self.y_min
+            return occ_map.astype(np.uint8).tolist(), xcells, ycells, self.x_min, self.y_min
 
         (self.x_min, self.y_min, self.x_max, self.y_max) = self._calc_grid_bds(pts)
 
@@ -268,7 +268,7 @@ def get_occupancy_map(self):
         pts = pts[:, 1] * xcells + pts[:, 0]
         occ_map = np.zeros(xcells * ycells)
         occ_map[pts] = self.obstacle_cost
-        return occ_map, xcells, ycells, self.x_min, self.y_min
+        return occ_map.astype(np.uint8).tolist(), xcells, ycells, self.x_min, self.y_min
 
     def _pub_occupancy_map(self):
         occ_map, width, height, x_min, y_min = self.get_occupancy_map()
@@ -367,7 +367,7 @@ def get_link_transform(self, src, tgt):
     def _init_occupancy_map(self):
         grid = OccupancyGrid()
         grid.header.seq = 1
-        grid.header.frame_id = "/map"
+        grid.header.frame_id = "map"
         grid.info.origin.position.z = 0
         grid.info.origin.orientation.x = 0
         grid.info.origin.orientation.y = 0

src/pyrobot/cfg/locobot_config.py

@@ -122,9 +122,9 @@
 # reference link name of the visual SLAM system, the pose of this frame
 # in the first time step will be used to define
 # the origin/orientation of the world frame
-_BASEC.VSLAM.VSLAM_BASE_FRAME = "/base_link"
+_BASEC.VSLAM.VSLAM_BASE_FRAME = "base_link"
 # RGB camera center frame name
-_BASEC.VSLAM.RGB_CAMERA_CENTER_FRAME = "/camera_color_optical_frame"
+_BASEC.VSLAM.RGB_CAMERA_CENTER_FRAME = "camera_color_optical_frame"
 # minimum depth values to be considered as valid
 _BASEC.VSLAM.DEPTH_MIN = 0.2
 # maximum depth values to be considered as valid