diff --git a/.gitignore b/.gitignore index 5e84fbf5..93d46274 100644 --- a/.gitignore +++ b/.gitignore @@ -155,6 +155,7 @@ thirdparty/ORB_SLAM2/Vocabulary/ORBvoc.txt # tmp files for testing tmp/* - +*.swo +*.swp # docusaurus node_modules diff --git a/docker/calibrated.json b/docker/calibrated.json new file mode 100644 index 00000000..2e5d9f70 --- /dev/null +++ b/docker/calibrated.json @@ -0,0 +1,18 @@ +{ + "camera_link": { + "from": "head_tilt_link", + "quat": [ + 0.0020485991725649184, + -0.02249425053471297, + 0.0001768645209827914, + 0.9997448577778497 + ], + "to": "camera_link", + "trans": [ + 0.0545564563198051, + 0.01111847207083916, + 0.004833795657717313 + ] + } +} + diff --git a/docker/docker_build.py b/docker/docker_build.py index 344e92f2..eaef5ae6 100644 --- a/docker/docker_build.py +++ b/docker/docker_build.py @@ -22,7 +22,7 @@ def execute_build(args): # copy requirements file from parent into docker folder cmd = 'DOCKER_BUILDKIT=1 docker build ' - cmd += '--ssh default ' + #cmd += '--ssh default ' cmd += '--network=host ' if args.no_cache: cmd += '--no-cache ' diff --git a/docker/docker_build_cuda.py b/docker/docker_build_cuda.py new file mode 100644 index 00000000..ef3e82e4 --- /dev/null +++ b/docker/docker_build_cuda.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import argparse +import os +import shutil + + +def execute_build(args): + """ + Create docker build command based on user input arguments + and execute the command + + Args: + args (argparse.Namespace): Build arguments + """ + if not os.path.exists(args.docker_file): + print('Dockerfile %s not found! Exiting' % args.docker_file) + return + + # copy requirements file from parent into docker folder + + cmd = 'DOCKER_BUILDKIT=1 docker build ' + #cmd += '--ssh default ' + cmd += '--network=host ' + if args.no_cache: + cmd += '--no-cache ' + cmd += '-t %s -f %s .' % (args.image, args.docker_file) + + print('command = \n\n', cmd) + + if not args.dry_run: + os.system(cmd) + + +if __name__ == '__main__': + default_image_name = "ros:noetic-ros-core-focal-cuda" + + parser = argparse.ArgumentParser() + + parser.add_argument('-i', '--image', type=str, + default=default_image_name, + help='name for new docker image') + + parser.add_argument('--no_cache', action='store_true', + help='0 if should build without using cache') + + parser.add_argument('-f', '--docker_file', type=str, + default='locobot_cuda.dockerfile', + help='which Dockerfile to build from') + + + parser.add_argument('-d', '--dry_run', action='store_true', + help='1 if we should only print the build command ' + 'without executing') + + args = parser.parse_args() + execute_build(args) diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh index fa7cddd9..0e1726e6 100755 --- a/docker/entrypoint.sh +++ b/docker/entrypoint.sh @@ -6,8 +6,8 @@ source /opt/ros/noetic/setup.bash source /root/create_ws/devel/setup.bash source /root/locobot_ws/devel/setup.bash source /root/.bashrc -cd workspace/ -pip install -e /workspace/pyrobot + +cd /workspace/ eval "bash" diff --git a/docker/install_scripts/install_locobot.sh b/docker/install_scripts/install_locobot.sh index 62362156..05986558 100644 --- a/docker/install_scripts/install_locobot.sh +++ b/docker/install_scripts/install_locobot.sh @@ -15,10 +15,21 @@ git clone https://github.com/turtlebot/turtlebot_interactions.git git clone https://github.com/turtlebot/turtlebot_simulator.git git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench.git git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git -git clone https://github.com/AutonomyLab/create_robot.git -git clone https://github.com/Improbable-AI/pyrobot.git +git clone --branch remove-orb2-slam https://github.com/Improbable-AI/pyrobot.git +git clone --branch indigo-devel https://github.com/AutonomyLab/create_autonomy.git +git clone https://github.com/Improbable-AI/airobot.git + + cd .. rosdep update apt-get update -rosdep install --from-paths src/create_robot src/dynamixel-workbench src/dynamixel-workbench-msgs src/ar_track_alvar -i -y -catkin build \ No newline at end of file +rosdep install --from-paths src/create_autonomy src/dynamixel-workbench src/dynamixel-workbench-msgs src/ar_track_alvar -i -y +catkin build + +cd src/pyrobot +pip install . +cd .. +cd airobot +pip install . + + diff --git a/docker/install_scripts/install_pytorch.sh b/docker/install_scripts/install_pytorch.sh index 64dfdd4f..e7dfc468 100644 --- a/docker/install_scripts/install_pytorch.sh +++ b/docker/install_scripts/install_pytorch.sh @@ -8,3 +8,4 @@ rm -rf /var/lib/apt/lists/* # install pytorch pip install torch==1.11.0+cpu -f https://download.pytorch.org/whl/cpu/torch_stable.html +pip install torchvision diff --git a/docker/install_scripts/install_pytorch_cuda.sh b/docker/install_scripts/install_pytorch_cuda.sh new file mode 100644 index 00000000..a71659b2 --- /dev/null +++ b/docker/install_scripts/install_pytorch_cuda.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +set -euxo pipefail + +rm -rf /var/lib/apt/lists/* + +# install pytotorch +pip install torch torchvision --extra-index-url https://download.pytorch.org/whl/cu113 +pip install tensorboard \ No newline at end of file diff --git a/docker/locobot.dockerfile b/docker/locobot.dockerfile index f9b0c70e..7316de1a 100644 --- a/docker/locobot.dockerfile +++ b/docker/locobot.dockerfile @@ -79,9 +79,13 @@ COPY install_scripts/install_py_pkgs2.sh /tmp/install_py_pkgs2.sh RUN chmod +x /tmp/install_py_pkgs2.sh RUN /tmp/install_py_pkgs2.sh +COPY calibrated.json /root/.robot/calibrated.json + RUN echo "source /opt/ros/noetic/setup.bash" >> /root/.bashrc RUN echo "source /root/create_ws/devel/setup.bash" >> /root/.bashrc RUN echo "source /root/locobot_ws/devel/setup.bash" >> /root/.bashrc +RUN echo "export ROS_IP=192.168.50.117" >> /root/.bashrc +RUN echo "export ROS_MASTER_URI=http://192.168.50.117:11311" >> /root/.bashrc WORKDIR / diff --git a/docker/locobot_cuda.dockerfile b/docker/locobot_cuda.dockerfile new file mode 100644 index 00000000..04c7178e --- /dev/null +++ b/docker/locobot_cuda.dockerfile @@ -0,0 +1,104 @@ +# syntax=docker/dockerfile:experimental + +FROM nvidia/cudagl:11.4.2-devel-ubuntu20.04 + +RUN echo 'America/New_York' > /etc/timezone && \ + ln -s /usr/share/zoneinfo/America/New_York /etc/localtime && \ + apt-get update && \ + apt-get install -q -y --no-install-recommends tzdata && \ + rm -rf /var/lib/apt/lists/* + +### second stage ### +ENV LC_ALL C.UTF-8 +ENV LANG C.UTF-8 + +# Replacing shell with bash for later docker build commands +RUN mv /bin/sh /bin/sh-old && \ + ln -s /bin/bash /bin/sh + +# install basic system stuff +COPY ./install_scripts/install_basic.sh /tmp/install_basic.sh +RUN chmod +x /tmp/install_basic.sh +RUN /tmp/install_basic.sh + + +# install ROS stuff +ENV ROS_DISTRO noetic + + + +COPY ./install_scripts/install_python.sh /tmp/install_python.sh +RUN chmod +x /tmp/install_python.sh +RUN /tmp/install_python.sh + +ARG DEBIAN_FRONTEND=noninteractive + +# create catkin workspace + + +COPY ./install_scripts/install_realsense.sh /tmp/install_realsense.sh +RUN chmod +x /tmp/install_realsense.sh +RUN /tmp/install_realsense.sh + +COPY ./install_scripts/install_ros.sh /tmp/install_ros.sh +RUN chmod +x /tmp/install_ros.sh +RUN /tmp/install_ros.sh + +# bootstrap rosdep +RUN rosdep init +RUN rosdep update + + +ENV CATKIN_WS=/root/catkin_ws +RUN source /opt/ros/$ROS_DISTRO/setup.bash + + +COPY ./install_scripts/install_pytorch_cuda.sh /tmp/install_pytorch_cuda.sh +RUN chmod +x /tmp/install_pytorch_cuda.sh +RUN /tmp/install_pytorch_cuda.sh + +# copy local requirements file for pip install python deps +ENV IMPROB /workspace +RUN mkdir ${IMPROB} + + +COPY install_scripts/install_py_pkgs.sh /tmp/install_py_pkgs.sh +RUN chmod +x /tmp/install_py_pkgs.sh +RUN /tmp/install_py_pkgs.sh + +COPY ./install_scripts/install_libcreate.sh /tmp/install_libcreate.sh +RUN chmod +x /tmp/install_libcreate.sh +RUN /tmp/install_libcreate.sh + + +COPY ./install_scripts/install_locobot.sh /tmp/install_locobot.sh +RUN chmod +x /tmp/install_locobot.sh +RUN /tmp/install_locobot.sh + +COPY install_scripts/install_py_pkgs2.sh /tmp/install_py_pkgs2.sh +RUN chmod +x /tmp/install_py_pkgs2.sh +RUN /tmp/install_py_pkgs2.sh + +COPY calibrated.json /root/.robot/calibrated.json + +RUN echo "source /opt/ros/noetic/setup.bash" >> /root/.bashrc +RUN echo "source /root/create_ws/devel/setup.bash" >> /root/.bashrc +RUN echo "source /root/locobot_ws/devel/setup.bash" >> /root/.bashrc +RUN echo "export ROS_MASTER_URI=http://192.168.50.117:11311" >> /root/.bashrc +RUN echo "export ROS_IP=192.168.50.111" >> /root/.bashrc + + +WORKDIR / +# Exposing the ports +EXPOSE 11311 + + +# setup entrypoint +COPY ./entrypoint.sh / + +ENTRYPOINT ["./entrypoint.sh"] +CMD ["bash"] + + + + diff --git a/docker/run_image_cuda.bash b/docker/run_image_cuda.bash new file mode 100755 index 00000000..0f5e1128 --- /dev/null +++ b/docker/run_image_cuda.bash @@ -0,0 +1,14 @@ +IMAGE=ros:noetic-ros-core-focal-cuda +docker run -it\ + --env="DISPLAY" \ + --env="QT_X11_NO_MITSHM=1" \ + --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ + --volume="$PWD/../../:/workspace/" \ + -v /dev:/dev \ + --gpus all \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e NVIDIA_VISIBLE_DEVICES=all \ + --privileged \ + --net=host \ + ${IMAGE} + diff --git a/pyrobot_bridge/nodes/kinematics.py b/pyrobot_bridge/nodes/kinematics.py index 8ec59c36..13fb89f8 100755 --- a/pyrobot_bridge/nodes/kinematics.py +++ b/pyrobot_bridge/nodes/kinematics.py @@ -24,176 +24,176 @@ class Kinematics(object): - """docstring for Kinematics""" - def __init__(self): - self.init_done = False - rospy.init_node('pyrobot_kinematics') - - self.ik_srv = rospy.Service('pyrobot/ik', IkCommand, self.ik_server) - self.fk_srv = rospy.Service('pyrobot/fk', FkCommand, self.fk_server) - rospy.spin() - - def _init_kinematics(self): - # Ros-Params - base_link = rospy.get_param('pyrobot/base_link') - gripper_link = rospy.get_param('pyrobot/gripper_link') - robot_description = rospy.get_param('pyrobot/robot_description') - - urdf_string = rospy.get_param(robot_description) - self.num_ik_solver = trac_ik.IK(base_link, - gripper_link, - urdf_string=urdf_string) - - _, self.urdf_tree = treeFromParam(robot_description) - self.urdf_chain = self.urdf_tree.getChain(base_link, - gripper_link) - self.arm_joint_names = self._get_kdl_joint_names() - self.arm_link_names = self._get_kdl_link_names() - self.arm_dof = len(self.arm_joint_names) - - self.jac_solver = kdl.ChainJntToJacSolver(self.urdf_chain) - self.fk_solver_pos = kdl.ChainFkSolverPos_recursive(self.urdf_chain) - self.fk_solver_vel = kdl.ChainFkSolverVel_recursive(self.urdf_chain) - - def _get_kdl_link_names(self): - num_links = self.urdf_chain.getNrOfSegments() - link_names = [] - for i in range(num_links): - link_names.append(self.urdf_chain.getSegment(i).getName()) - return copy.deepcopy(link_names) - - def _get_kdl_joint_names(self): - num_links = self.urdf_chain.getNrOfSegments() - num_joints = self.urdf_chain.getNrOfJoints() - joint_names = [] - for i in range(num_links): - link = self.urdf_chain.getSegment(i) - joint = link.getJoint() - joint_type = joint.getType() - # JointType definition: [RotAxis,RotX,RotY,RotZ,TransAxis, - # TransX,TransY,TransZ,None] - if joint_type > 1: - continue - joint_names.append(joint.getName()) - assert num_joints == len(joint_names) - return copy.deepcopy(joint_names) - - def ik_server(self, req): - - if not self.init_done: - self._init_kinematics() - self.init_done = True - - resp = IkCommandResponse() - - if len(req.pose) < 7 or len(req.tolerance) < 6 \ - or len(req.init_joint_positions) != self.arm_dof : - resp.success = False - rospy.logerr("Incorrect IK service request. Please fix it.") - return resp - - joint_positions = self.num_ik_solver.get_ik(req.init_joint_positions, - req.pose[0], - req.pose[1], - req.pose[2], - req.pose[3], - req.pose[4], - req.pose[5], - req.pose[6], - req.tolerance[0], - req.tolerance[1], - req.tolerance[2], - req.tolerance[3], - req.tolerance[4], - req.tolerance[5],) - - if joint_positions is None: - resp.success = False - rospy.logwarn("Failed to find find an IK solution.") - return resp - - resp.joint_positions = joint_positions - resp.success = True - - return resp - - def _kdl_frame_to_numpy(self, frame): - p = frame.p - M = frame.M - return np.array([[M[0, 0], M[0, 1], M[0, 2], p.x()], - [M[1, 0], M[1, 1], M[1, 2], p.y()], - [M[2, 0], M[2, 1], M[2, 2], p.z()], - [0, 0, 0, 1]]) - - def rot_mat_to_quat(self, rot): - """ - Convert the rotation matrix into quaternion. - :param quat: the rotation matrix (shape: :math:`[3, 3]`) - :type quat: numpy.ndarray - :return: quaternion [x, y, z, w] (shape: :math:`[4,]`) - :rtype: numpy.ndarray - """ - R = np.eye(4) - R[:3, :3] = rot - return tf.transformations.quaternion_from_matrix(R) - - def joints_to_kdl(self, joint_values): - """ - Convert the numpy array into KDL data format - :param joint_values: values for the joints - :return: kdl data type - """ - num_jts = joint_values.size - kdl_array = kdl.JntArray(num_jts) - for idx in range(num_jts): - kdl_array[idx] = joint_values[idx] - return kdl_array - - def fk_server(self, req): - """ - Given joint angles, compute the pose of desired_frame with respect - to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame - must be in self.arm_link_names - :param joint_positions: joint angles - :param des_frame: desired frame - :type joint_positions: np.ndarray - :type des_frame: string - :return: translational vector and rotational matrix - :rtype: np.ndarray, np.ndarray - """ - if not self.init_done: - self._init_kinematics() - self.init_done = True - - resp = FkCommandResponse() - - joint_positions = np.asarray(req.joint_angles).flatten() - des_frame = req.end_frame - if joint_positions.size != self.arm_dof: - rospy.logerr('Forward Kinenatics: Invalid length of joint angles!') - resp.success = False - return resp - - kdl_jnt_angles = self.joints_to_kdl(joint_positions) - - - kdl_end_frame = kdl.Frame() - idx = self.arm_link_names.index(des_frame) + 1 - fg = self.fk_solver_pos.JntToCart(kdl_jnt_angles, - kdl_end_frame, - idx) - - if fg < 0: - rospy.logerr('Forward Kinenatics: KDL Pos JntToCart error!') - resp.success = False - return resp - - pose = self._kdl_frame_to_numpy(kdl_end_frame) - pos = pose[:3, 3].reshape(-1, 1) - resp.pos = list(pos) - resp.quat = list(self.rot_mat_to_quat(pose[:3, :3])) - resp.success = True - return resp + """docstring for Kinematics""" + def __init__(self): + self.init_done = False + rospy.init_node('pyrobot_kinematics') + + self.ik_srv = rospy.Service('pyrobot/ik', IkCommand, self.ik_server) + self.fk_srv = rospy.Service('pyrobot/fk', FkCommand, self.fk_server) + rospy.spin() + + def _init_kinematics(self): + # Ros-Params + base_link = rospy.get_param('pyrobot/base_link') + gripper_link = rospy.get_param('pyrobot/gripper_link') + robot_description = rospy.get_param('pyrobot/robot_description') + + urdf_string = rospy.get_param(robot_description) + self.num_ik_solver = trac_ik.IK(base_link, + gripper_link, + urdf_string=urdf_string) + + _, self.urdf_tree = treeFromParam(robot_description) + self.urdf_chain = self.urdf_tree.getChain(base_link, + gripper_link) + self.arm_joint_names = self._get_kdl_joint_names() + self.arm_link_names = self._get_kdl_link_names() + self.arm_dof = len(self.arm_joint_names) + + self.jac_solver = kdl.ChainJntToJacSolver(self.urdf_chain) + self.fk_solver_pos = kdl.ChainFkSolverPos_recursive(self.urdf_chain) + self.fk_solver_vel = kdl.ChainFkSolverVel_recursive(self.urdf_chain) + + def _get_kdl_link_names(self): + num_links = self.urdf_chain.getNrOfSegments() + link_names = [] + for i in range(num_links): + link_names.append(self.urdf_chain.getSegment(i).getName()) + return copy.deepcopy(link_names) + + def _get_kdl_joint_names(self): + num_links = self.urdf_chain.getNrOfSegments() + num_joints = self.urdf_chain.getNrOfJoints() + joint_names = [] + for i in range(num_links): + link = self.urdf_chain.getSegment(i) + joint = link.getJoint() + joint_type = joint.getType() + # JointType definition: [RotAxis,RotX,RotY,RotZ,TransAxis, + # TransX,TransY,TransZ,None] + if joint_type > 1: + continue + joint_names.append(joint.getName()) + assert num_joints == len(joint_names) + return copy.deepcopy(joint_names) + + def ik_server(self, req): + + if not self.init_done: + self._init_kinematics() + self.init_done = True + + resp = IkCommandResponse() + + if len(req.pose) < 7 or len(req.tolerance) < 6 \ + or len(req.init_joint_positions) != self.arm_dof : + resp.success = False + rospy.logerr("Incorrect IK service request. Please fix it.") + return resp + + joint_positions = self.num_ik_solver.get_ik(req.init_joint_positions, + req.pose[0], + req.pose[1], + req.pose[2], + req.pose[3], + req.pose[4], + req.pose[5], + req.pose[6], + req.tolerance[0], + req.tolerance[1], + req.tolerance[2], + req.tolerance[3], + req.tolerance[4], + req.tolerance[5],) + + if joint_positions is None: + resp.success = False + rospy.logwarn("Failed to find find an IK solution.") + return resp + + resp.joint_positions = joint_positions + resp.success = True + + return resp + + def _kdl_frame_to_numpy(self, frame): + p = frame.p + M = frame.M + return np.array([[M[0, 0], M[0, 1], M[0, 2], p.x()], + [M[1, 0], M[1, 1], M[1, 2], p.y()], + [M[2, 0], M[2, 1], M[2, 2], p.z()], + [0, 0, 0, 1]]) + + def rot_mat_to_quat(self, rot): + """ + Convert the rotation matrix into quaternion. + :param quat: the rotation matrix (shape: :math:`[3, 3]`) + :type quat: numpy.ndarray + :return: quaternion [x, y, z, w] (shape: :math:`[4,]`) + :rtype: numpy.ndarray + """ + R = np.eye(4) + R[:3, :3] = rot + return tf.transformations.quaternion_from_matrix(R) + + def joints_to_kdl(self, joint_values): + """ + Convert the numpy array into KDL data format + :param joint_values: values for the joints + :return: kdl data type + """ + num_jts = joint_values.size + kdl_array = kdl.JntArray(num_jts) + for idx in range(num_jts): + kdl_array[idx] = joint_values[idx] + return kdl_array + + def fk_server(self, req): + """ + Given joint angles, compute the pose of desired_frame with respect + to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame + must be in self.arm_link_names + :param joint_positions: joint angles + :param des_frame: desired frame + :type joint_positions: np.ndarray + :type des_frame: string + :return: translational vector and rotational matrix + :rtype: np.ndarray, np.ndarray + """ + if not self.init_done: + self._init_kinematics() + self.init_done = True + + resp = FkCommandResponse() + + joint_positions = np.asarray(req.joint_angles).flatten() + des_frame = req.end_frame + if joint_positions.size != self.arm_dof: + rospy.logerr('Forward Kinenatics: Invalid length of joint angles!') + resp.success = False + return resp + + kdl_jnt_angles = self.joints_to_kdl(joint_positions) + + + kdl_end_frame = kdl.Frame() + idx = self.arm_link_names.index(des_frame) + 1 + fg = self.fk_solver_pos.JntToCart(kdl_jnt_angles, + kdl_end_frame, + idx) + + if fg < 0: + rospy.logerr('Forward Kinenatics: KDL Pos JntToCart error!') + resp.success = False + return resp + + pose = self._kdl_frame_to_numpy(kdl_end_frame) + pos = pose[:3, 3].reshape(-1, 1) + resp.pos = list(pos) + resp.quat = list(self.rot_mat_to_quat(pose[:3, :3])) + resp.success = True + return resp if __name__ == "__main__": server = Kinematics() \ No newline at end of file diff --git a/robots/LoCoBot/locobot_calibration/CMakeLists.txt b/robots/LoCoBot/locobot_calibration/CMakeLists.txt index 739a2127..46081254 100644 --- a/robots/LoCoBot/locobot_calibration/CMakeLists.txt +++ b/robots/LoCoBot/locobot_calibration/CMakeLists.txt @@ -201,4 +201,4 @@ include_directories( # endif() ## Add folders to be run by python nosetests -# catkin_add_nosetests(test) \ No newline at end of file +# catkin_add_nosetests(test) diff --git a/robots/LoCoBot/locobot_control/launch/main.launch b/robots/LoCoBot/locobot_control/launch/main.launch index d249dbc7..856107be 100644 --- a/robots/LoCoBot/locobot_control/launch/main.launch +++ b/robots/LoCoBot/locobot_control/launch/main.launch @@ -31,6 +31,8 @@ + + diff --git a/robots/LoCoBot/locobot_control/launch/third_person_cam.launch b/robots/LoCoBot/locobot_control/launch/third_person_cam.launch new file mode 100644 index 00000000..40af67f8 --- /dev/null +++ b/robots/LoCoBot/locobot_control/launch/third_person_cam.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/src/pyrobot/cfg/locobot_config.py b/src/pyrobot/cfg/locobot_config.py index d456e693..f59f709f 100644 --- a/src/pyrobot/cfg/locobot_config.py +++ b/src/pyrobot/cfg/locobot_config.py @@ -45,6 +45,16 @@ _CAMERAC.ROSTOPIC_SET_PAN = '/pan/command' # topic name to set tilt angle _CAMERAC.ROSTOPIC_SET_TILT = '/tilt/command' +# Camera parameters +_CAMERAC.FX = 616.5984497070312 +_CAMERAC.FY = 616.4833374023438 +_CAMERAC.CX = 323.9175109863281 +_CAMERAC.CY = 239.705535888671885 +_CAMERAC.HEIGHT = 480 +_CAMERAC.WIDTH = 640 +# Depthmap values factor +_CAMERAC.DEPTH_MAP_FACTOR = 1000.0 + _BASEC = _C.BASE # BASE class name diff --git a/src/pyrobot/locobot/camera.py b/src/pyrobot/locobot/camera.py index 0d243b43..7822dfd0 100644 --- a/src/pyrobot/locobot/camera.py +++ b/src/pyrobot/locobot/camera.py @@ -78,10 +78,9 @@ def __init__(self, configs): self.sync.registerCallback(self._sync_callback) depth_threshold = (self.configs.BASE.VSLAM.DEPTH_MIN, self.configs.BASE.VSLAM.DEPTH_MAX) - cfg_filename = self.configs.BASE.VSLAM.CFG_FILENAME self.depth_cam = DepthImgProcessor(subsample_pixs=1, - depth_threshold=depth_threshold, - cfg_filename=cfg_filename) + configs = self.configs, + depth_threshold=depth_threshold) self.cam_cf = self.configs.BASE.VSLAM.RGB_CAMERA_CENTER_FRAME self.base_f = self.configs.BASE.VSLAM.VSLAM_BASE_FRAME @@ -437,8 +436,7 @@ class DepthImgProcessor: This class transforms the depth image and rgb image to point cloud """ - def __init__(self, subsample_pixs=1, depth_threshold=(0, 1.5), - cfg_filename='realsense_d435.yaml'): + def __init__(self, configs, subsample_pixs=1, depth_threshold=(0, 1.5)): """ The constructor for :class:`DepthImgProcessor` class. @@ -453,14 +451,15 @@ def __init__(self, subsample_pixs=1, depth_threshold=(0, 1.5), assert (type(depth_threshold) is tuple and 0 < len(depth_threshold) < 3) or \ (depth_threshold is None) + self.configs = configs self.subsample_pixs = subsample_pixs self.depth_threshold = depth_threshold - self.cfg_data = self.read_cfg(cfg_filename) self.intrinsic_mat = self.get_intrinsic() self.intrinsic_mat_inv = np.linalg.inv(self.intrinsic_mat) + - img_pixs = np.mgrid[0: self.cfg_data['Camera.height']: subsample_pixs, - 0: self.cfg_data['Camera.width']: subsample_pixs] + img_pixs = np.mgrid[0: self.configs.CAMERA.HEIGHT: subsample_pixs, + 0: self.configs.CAMERA.WIDTH: subsample_pixs] img_pixs = img_pixs.reshape(2, -1) img_pixs[[0, 1], :] = img_pixs[[1, 0], :] self.uv_one = np.concatenate((img_pixs, @@ -502,7 +501,7 @@ def get_pix_3dpt(self, depth_im, rs, cs): if isinstance(cs, np.ndarray): cs = cs.flatten() depth_im = depth_im[rs, cs] - depth = depth_im.reshape(-1) / float(self.cfg_data['DepthMapFactor']) + depth = depth_im.reshape(-1) / float(self.configs.CAMERA.DEPTH_MAP_FACTOR) img_pixs = np.stack((rs, cs)).reshape(2, -1) img_pixs[[0, 1], :] = img_pixs[[1, 0], :] uv_one = np.concatenate((img_pixs, @@ -537,7 +536,7 @@ def get_pcd_ic(self, depth_im, rgb_im=None): # pcd in camera from depth depth_im = depth_im[0::self.subsample_pixs, 0::self.subsample_pixs] rgb_im = rgb_im[0::self.subsample_pixs, 0::self.subsample_pixs] - depth = depth_im.reshape(-1) / float(self.cfg_data['DepthMapFactor']) + depth = depth_im.reshape(-1) / float(self.configs.CAMERA.DEPTH_MAP_FACTOR) rgb = None if rgb_im is not None: rgb = rgb_im.reshape(-1, 3) @@ -578,8 +577,9 @@ def get_pcd_iw(self, pts_in_cam, extrinsic_mat): pts_in_world = pts_in_world[:3, :].T return pts_in_world + """ def read_cfg(self, cfg_filename): - """ + Reads the configuration file :param cfg_filename: configuration file name for ORB-SLAM2 @@ -588,7 +588,7 @@ def read_cfg(self, cfg_filename): :return: configurations in the configuration file :rtype: dict - """ + rospack = rospkg.RosPack() slam_pkg_path = rospack.get_path('orb_slam2_ros') cfg_path = os.path.join(slam_pkg_path, @@ -599,6 +599,7 @@ def read_cfg(self, cfg_filename): f.readline() cfg_data = yaml.load(f) return cfg_data + """ def get_intrinsic(self): """ @@ -607,10 +608,10 @@ def get_intrinsic(self): :return: the intrinsic matrix (shape: :math:`[3, 3]`) :rtype: np.ndarray """ - fx = self.cfg_data['Camera.fx'] - fy = self.cfg_data['Camera.fy'] - cx = self.cfg_data['Camera.cx'] - cy = self.cfg_data['Camera.cy'] + fx = self.configs.CAMERA.FX + fy = self.configs.CAMERA.FY + cx = self.configs.CAMERA.CX + cy = self.configs.CAMERA.CY Itc = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])