From 91c67477823e10c1f77277fb72f18cdd48d03702 Mon Sep 17 00:00:00 2001 From: Zisu Dong Date: Wed, 3 Feb 2021 12:11:37 -0500 Subject: [PATCH 1/7] namespace fix --- .../locobot_control/launch/vslam_debug.launch | 122 ++++++++++++++++++ .../src/locobot_controller.cpp | 2 +- .../global_costmap_params_with_map.yaml | 4 +- .../base_navigation/nodes/mapToGrid.py | 1 + .../scripts/orb_slam2_ros/pcdlib.py | 4 +- .../scripts/orb_slam2_ros/vslam.py | 6 +- src/pyrobot/cfg/locobot_config.py | 4 +- 7 files changed, 133 insertions(+), 10 deletions(-) create mode 100644 robots/LoCoBot/locobot_control/launch/vslam_debug.launch diff --git a/robots/LoCoBot/locobot_control/launch/vslam_debug.launch b/robots/LoCoBot/locobot_control/launch/vslam_debug.launch new file mode 100644 index 00000000..6ea9d528 --- /dev/null +++ b/robots/LoCoBot/locobot_control/launch/vslam_debug.launch @@ -0,0 +1,122 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robots/LoCoBot/locobot_control/src/locobot_controller.cpp b/robots/LoCoBot/locobot_control/src/locobot_controller.cpp index 0932b96f..918512fe 100644 --- a/robots/LoCoBot/locobot_control/src/locobot_controller.cpp +++ b/robots/LoCoBot/locobot_control/src/locobot_controller.cpp @@ -73,7 +73,7 @@ LoCoBotController::~LoCoBotController() { if (use_group_["arm"]) { if (!torque_control_) { std::map 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]; diff --git a/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/global_costmap_params_with_map.yaml b/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/global_costmap_params_with_map.yaml index 70cf8c7b..f49c6f8c 100644 --- a/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/global_costmap_params_with_map.yaml +++ b/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 diff --git a/robots/LoCoBot/locobot_navigation/base_navigation/nodes/mapToGrid.py b/robots/LoCoBot/locobot_navigation/base_navigation/nodes/mapToGrid.py index 4687f2db..56160a0c 100755 --- a/robots/LoCoBot/locobot_navigation/base_navigation/nodes/mapToGrid.py +++ b/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() diff --git a/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/pcdlib.py b/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/pcdlib.py index 8fc4898b..d7b89b6a 100644 --- a/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/pcdlib.py +++ b/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] diff --git a/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/vslam.py b/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/vslam.py index be38b0fd..19dffe44 100644 --- a/robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/vslam.py +++ b/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 diff --git a/src/pyrobot/cfg/locobot_config.py b/src/pyrobot/cfg/locobot_config.py index bfcd6119..87b0b1db 100644 --- a/src/pyrobot/cfg/locobot_config.py +++ b/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 From f096066b42b79a9a239c04289b2233ffa282c35c Mon Sep 17 00:00:00 2001 From: Zisu Dong Date: Wed, 3 Feb 2021 19:14:03 -0500 Subject: [PATCH 2/7] fix pip ver --- robots/LoCoBot/install/locobot_install_all.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robots/LoCoBot/install/locobot_install_all.sh b/robots/LoCoBot/install/locobot_install_all.sh index a363bc46..ee763648 100755 --- a/robots/LoCoBot/install/locobot_install_all.sh +++ b/robots/LoCoBot/install/locobot_install_all.sh @@ -107,7 +107,7 @@ install_packages "${package_names[@]}" sudo pip install --upgrade cryptography sudo python -m easy_install --upgrade pyOpenSSL -sudo pip install --upgrade pip +sudo pip install --upgrade pip==20.3 # STEP 2 - Install ROS From eb376f7d1c0ca4e628608d8b974f867ab130074d Mon Sep 17 00:00:00 2001 From: Zisu Dong Date: Wed, 3 Feb 2021 19:15:48 -0500 Subject: [PATCH 3/7] branch link --- robots/LoCoBot/install/locobot_install_all.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robots/LoCoBot/install/locobot_install_all.sh b/robots/LoCoBot/install/locobot_install_all.sh index ee763648..f515abec 100755 --- a/robots/LoCoBot/install/locobot_install_all.sh +++ b/robots/LoCoBot/install/locobot_install_all.sh @@ -238,7 +238,7 @@ if [ ! -d "$LOCOBOT_FOLDER/src/pyrobot" ]; then cd $LOCOBOT_FOLDER/src git clone https://github.com/facebookresearch/pyrobot.git cd pyrobot - git checkout master + git checkout vslam_debug git submodule update --init --recursive if [ $LOCOBOT_PLATFORM == "cmu" ]; then cd $LOCOBOT_FOLDER/src/pyrobot/robots/LoCoBot/locobot_description/urdf From 96c43277bfd10afffbb22b1e7ec60a65ebdb3bdc Mon Sep 17 00:00:00 2001 From: Zisu Dong Date: Thu, 4 Feb 2021 13:54:27 -0500 Subject: [PATCH 4/7] fix vslam --- robots/LoCoBot/install/locobot_install_all.sh | 7 ++++--- robots/LoCoBot/locobot_control/launch/vslam_debug.launch | 6 +++--- .../base_navigation/config/turtlebot/global_map_extra.yaml | 2 +- .../config/turtlebot/local_costmap_params.yaml | 2 +- .../base_navigation/config/turtlebot/local_map_extra.yaml | 2 +- .../base_navigation/launch/turtlebot_move_base.launch | 4 ++-- 6 files changed, 12 insertions(+), 11 deletions(-) diff --git a/robots/LoCoBot/install/locobot_install_all.sh b/robots/LoCoBot/install/locobot_install_all.sh index f515abec..e051d7e5 100755 --- a/robots/LoCoBot/install/locobot_install_all.sh +++ b/robots/LoCoBot/install/locobot_install_all.sh @@ -268,7 +268,7 @@ if [ ! -d "$LOCOBOT_FOLDER/src/pyrobot/robots/LoCoBot/thirdparty" ]; then git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git git clone https://github.com/ros-controls/ros_control.git - git clone https://github.com/kalyanvasudev/ORB_SLAM2.git + git clone https://github.com/taochenshh/ORB_SLAM2.git git clone https://github.com/s-gupta/ar_track_alvar.git if [ $ROS_NAME == "kinetic" ]; then @@ -277,7 +277,6 @@ if [ ! -d "$LOCOBOT_FOLDER/src/pyrobot/robots/LoCoBot/thirdparty" ]; then cd DynamixelSDK && git checkout 05dcc5c551598b4d323bf1fb4b9d1ee03ad1dfd9 && cd .. cd dynamixel-workbench-msgs && git checkout 93856f5d3926e4d7a63055c04a3671872799cc86 && cd .. cd ros_control && git checkout 44cf68aab6cb1293e91f69ef7efe30b80195356b && cd .. - cd ORB_SLAM2 && git checkout ec8d750d3fc813fe5cef82f16d5cc11ddfc7bb3d && cd .. cd ar_track_alvar && git checkout 625a3cf928552ee5bf97453897af6790d523962f && cd .. else cd create_autonomy && git checkout 90e597ea4d85cde1ec32a1d43ea2dd0b4cbf481c && cd .. @@ -285,9 +284,11 @@ if [ ! -d "$LOCOBOT_FOLDER/src/pyrobot/robots/LoCoBot/thirdparty" ]; then cd DynamixelSDK && git checkout 05dcc5c551598b4d323bf1fb4b9d1ee03ad1dfd9 && cd .. cd dynamixel-workbench-msgs && git checkout 93856f5d3926e4d7a63055c04a3671872799cc86 && cd .. cd ros_control && git checkout cd39acfdb2d08dc218d04ff98856b0e6a525e702 && cd .. - cd ORB_SLAM2 && git checkout ec8d750d3fc813fe5cef82f16d5cc11ddfc7bb3d && cd .. cd ar_track_alvar && git checkout a870d5f00a548acb346bfcc89d42b997771d71a3 && cd .. fi + cd ORB_SLAM2 + chmod +x install.sh + sudo bash -i install.sh fi cd $LOCOBOT_FOLDER diff --git a/robots/LoCoBot/locobot_control/launch/vslam_debug.launch b/robots/LoCoBot/locobot_control/launch/vslam_debug.launch index 6ea9d528..01c3ed00 100644 --- a/robots/LoCoBot/locobot_control/launch/vslam_debug.launch +++ b/robots/LoCoBot/locobot_control/launch/vslam_debug.launch @@ -4,7 +4,7 @@ - + @@ -89,7 +89,7 @@ - + diff --git a/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/global_map_extra.yaml b/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/global_map_extra.yaml index 993bcc56..5f899c24 100644 --- a/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/global_map_extra.yaml +++ b/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: diff --git a/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/local_costmap_params.yaml b/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/local_costmap_params.yaml index b71c5ef8..cd51ed6c 100644 --- a/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/local_costmap_params.yaml +++ b/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 diff --git a/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/local_map_extra.yaml b/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/local_map_extra.yaml index 993bcc56..5f899c24 100644 --- a/robots/LoCoBot/locobot_navigation/base_navigation/config/turtlebot/local_map_extra.yaml +++ b/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: diff --git a/robots/LoCoBot/locobot_navigation/base_navigation/launch/turtlebot_move_base.launch b/robots/LoCoBot/locobot_navigation/base_navigation/launch/turtlebot_move_base.launch index c010bcb6..59e7e473 100644 --- a/robots/LoCoBot/locobot_navigation/base_navigation/launch/turtlebot_move_base.launch +++ b/robots/LoCoBot/locobot_navigation/base_navigation/launch/turtlebot_move_base.launch @@ -25,9 +25,9 @@ - + ns="global_costmap" /> --> From 5dbbf8cc02a128156ee8cb389ddeae62ba648297 Mon Sep 17 00:00:00 2001 From: Zisu Date: Thu, 4 Feb 2021 17:20:05 -0500 Subject: [PATCH 5/7] vslam tested --- robots/LoCoBot/install/locobot_install_all.sh | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/robots/LoCoBot/install/locobot_install_all.sh b/robots/LoCoBot/install/locobot_install_all.sh index e051d7e5..f515abec 100755 --- a/robots/LoCoBot/install/locobot_install_all.sh +++ b/robots/LoCoBot/install/locobot_install_all.sh @@ -268,7 +268,7 @@ if [ ! -d "$LOCOBOT_FOLDER/src/pyrobot/robots/LoCoBot/thirdparty" ]; then git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git git clone https://github.com/ros-controls/ros_control.git - git clone https://github.com/taochenshh/ORB_SLAM2.git + git clone https://github.com/kalyanvasudev/ORB_SLAM2.git git clone https://github.com/s-gupta/ar_track_alvar.git if [ $ROS_NAME == "kinetic" ]; then @@ -277,6 +277,7 @@ if [ ! -d "$LOCOBOT_FOLDER/src/pyrobot/robots/LoCoBot/thirdparty" ]; then cd DynamixelSDK && git checkout 05dcc5c551598b4d323bf1fb4b9d1ee03ad1dfd9 && cd .. cd dynamixel-workbench-msgs && git checkout 93856f5d3926e4d7a63055c04a3671872799cc86 && cd .. cd ros_control && git checkout 44cf68aab6cb1293e91f69ef7efe30b80195356b && cd .. + cd ORB_SLAM2 && git checkout ec8d750d3fc813fe5cef82f16d5cc11ddfc7bb3d && cd .. cd ar_track_alvar && git checkout 625a3cf928552ee5bf97453897af6790d523962f && cd .. else cd create_autonomy && git checkout 90e597ea4d85cde1ec32a1d43ea2dd0b4cbf481c && cd .. @@ -284,11 +285,9 @@ if [ ! -d "$LOCOBOT_FOLDER/src/pyrobot/robots/LoCoBot/thirdparty" ]; then cd DynamixelSDK && git checkout 05dcc5c551598b4d323bf1fb4b9d1ee03ad1dfd9 && cd .. cd dynamixel-workbench-msgs && git checkout 93856f5d3926e4d7a63055c04a3671872799cc86 && cd .. cd ros_control && git checkout cd39acfdb2d08dc218d04ff98856b0e6a525e702 && cd .. + cd ORB_SLAM2 && git checkout ec8d750d3fc813fe5cef82f16d5cc11ddfc7bb3d && cd .. cd ar_track_alvar && git checkout a870d5f00a548acb346bfcc89d42b997771d71a3 && cd .. fi - cd ORB_SLAM2 - chmod +x install.sh - sudo bash -i install.sh fi cd $LOCOBOT_FOLDER From c349670ef8746a41e4fc7f2c2da2fca1b447dc12 Mon Sep 17 00:00:00 2001 From: Zisu Dong Date: Fri, 5 Feb 2021 15:30:27 -0500 Subject: [PATCH 6/7] Update install script for merging into master --- robots/LoCoBot/install/locobot_install_all.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robots/LoCoBot/install/locobot_install_all.sh b/robots/LoCoBot/install/locobot_install_all.sh index f515abec..ee763648 100755 --- a/robots/LoCoBot/install/locobot_install_all.sh +++ b/robots/LoCoBot/install/locobot_install_all.sh @@ -238,7 +238,7 @@ if [ ! -d "$LOCOBOT_FOLDER/src/pyrobot" ]; then cd $LOCOBOT_FOLDER/src git clone https://github.com/facebookresearch/pyrobot.git cd pyrobot - git checkout vslam_debug + git checkout master git submodule update --init --recursive if [ $LOCOBOT_PLATFORM == "cmu" ]; then cd $LOCOBOT_FOLDER/src/pyrobot/robots/LoCoBot/locobot_description/urdf From 69d857a6bbc867730215eaf977e28fd55316adf2 Mon Sep 17 00:00:00 2001 From: Zisu Dong Date: Fri, 5 Feb 2021 15:30:37 -0500 Subject: [PATCH 7/7] Update install script for merging into master