diff --git a/.gitignore b/.gitignore index ae8578f..46b93f8 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,5 @@ devel/ .catkin_tools logs __pycache__/ +tags +*.pyc diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..0765cab --- /dev/null +++ b/.gitmodules @@ -0,0 +1,9 @@ +[submodule "low_cost_ws/src/apriltags_ros_test"] + path = low_cost_ws/src/apriltags_ros_test + url = git@github.com:Sensing-Intelligent-System/apriltags_ros_test.git +[submodule "low_cost_ws/src/apriltags_ros"] + path = low_cost_ws/src/apriltags_ros + url = git@github.com:Sensing-Intelligent-System/apriltags_ros.git +[submodule "low_cost_ws/src/arg_utils"] + path = low_cost_ws/src/arg_utils + url = git@github.com:ARG-NCTU/arg_utils.git diff --git a/README.md b/README.md new file mode 100644 index 0000000..d9673eb --- /dev/null +++ b/README.md @@ -0,0 +1,22 @@ +# LoCoBot-RSA + +This is the class repo for Design and Implementation of Robotic Systems and Applications. + +## Installation + +By default we use native Ubuntu 20.04 (or dual boot if you are using Windows). + +### Several tools are required + +Git: please follow +* SSH Key: https://github.com/ARG-NCTU/oop-python-nycu/blob/main/tutorials/00-ssh-key.md + +### Some tools are recommended +* NeoVim and GitHub Copilot: https://github.com/ARG-NCTU/oop-python-nycu/blob/main/tutorials/04-copilot-neovim-nodejs.md + +## To Get Started + +We will use a docker image with ROS noetic. +``` +source docker_run.sh +``` diff --git a/docker/build.bash b/docker/build.bash index c44728d..ded1f8a 100644 --- a/docker/build.bash +++ b/docker/build.bash @@ -1,7 +1,7 @@ #!/usr/bin/env bash -REPOSITORY="sunfuchou/rsa" -TAG="amd64" +REPOSITORY="argnctu/rsa" +TAG="latest" IMG="${REPOSITORY}:${TAG}" diff --git a/docker/docker_join.bash b/docker/docker_join.bash index 341574c..0f103da 100644 --- a/docker/docker_join.bash +++ b/docker/docker_join.bash @@ -1,8 +1,8 @@ #!/usr/bin/env bash NAME=locobot_rsa -REPOSITORY="sunfuchou/rsa" -TAG="amd64" +REPOSITORY="argnctu/rsa" +TAG="latest" REPO_NAME=LoCoBot-RSA IMG="${REPOSITORY}:${TAG}" diff --git a/docker/docker_run.bash b/docker/docker_run.bash index 7779856..f223f4f 100644 --- a/docker/docker_run.bash +++ b/docker/docker_run.bash @@ -3,8 +3,8 @@ ARGS=("$@") NAME=rsa -REPOSITORY="sunfuchou/rsa" -TAG="amd64" +REPOSITORY="argnctu/rsa" +TAG="latest" REPO_NAME=LoCoBot-RSA IMG="${REPOSITORY}:${TAG}" diff --git a/docker/dockerfile b/docker/dockerfile index edbc743..09ded21 100644 --- a/docker/dockerfile +++ b/docker/dockerfile @@ -25,33 +25,44 @@ RUN echo "${USER}:rsa" | chpasswd RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ && apt-get -o Acquire::ForceIPv4=true install -yq --no-install-recommends \ - locales \ - curl \ - cmake \ - make \ - git \ - vim \ - wget \ - sudo \ - lsb-release \ - build-essential \ - net-tools \ - apt-utils \ - software-properties-common \ - dialog \ - libffi-dev \ - python3-dev \ - python3-pip \ - python3-setuptools \ - apt-transport-https \ - libglew-dev + locales \ + curl \ + cmake \ + make \ + git \ + vim \ + wget \ + sudo \ + lsb-release \ + build-essential \ + net-tools \ + apt-utils \ + software-properties-common \ + dialog \ + libffi-dev \ + python3-dev \ + python3-pip \ + python3-setuptools \ + apt-transport-https \ + libglew-dev \ + python3-tk RUN pip3 install --upgrade pip \ && pip3 install --upgrade setuptools \ - && pip3 install python_tsp pypozyx + && pip3 install \ + python_tsp \ + pypozyx \ + pytest \ + scipy \ + opencv-python \ + dbg \ + pytransform3d \ + wget \ + apriltag + RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ - && apt-get -o Acquire::ForceIPv4=true install -yq --no-install-recommends \ - ros-${ROS_DISTRO}-robot-localization +&& apt-get -o Acquire::ForceIPv4=true install -yq --no-install-recommends \ +ros-${ROS_DISTRO}-robot-localization RUN echo 'source /opt/ros/noetic/setup.bash' >> ~/.bashrc diff --git a/docker/push.bash b/docker/push.bash new file mode 100644 index 0000000..10bc484 --- /dev/null +++ b/docker/push.bash @@ -0,0 +1,3 @@ +#!/usr/bin/env bash + +docker image push argnctu/rsa:latest diff --git a/docker_join.bash b/docker_join.bash new file mode 120000 index 0000000..6fb77d8 --- /dev/null +++ b/docker_join.bash @@ -0,0 +1 @@ +docker/docker_join.bash \ No newline at end of file diff --git a/docker_run.bash b/docker_run.bash new file mode 120000 index 0000000..804015c --- /dev/null +++ b/docker_run.bash @@ -0,0 +1 @@ +docker/docker_run.bash \ No newline at end of file diff --git a/locobot/README.md b/locobot/README.md new file mode 100644 index 0000000..1dc93d5 --- /dev/null +++ b/locobot/README.md @@ -0,0 +1,7 @@ +# Code to Run within a LoCoBot + +The code in this folder is supposed to run on NYCU LoCoBot + +## NYCU LoCoBot Settings + +The settings follows the orignial version distributed with the hardware. diff --git a/locobot/environment.sh b/locobot/environment.sh index d868cd4..d45a2c0 100644 --- a/locobot/environment.sh +++ b/locobot/environment.sh @@ -1,5 +1,33 @@ #! /bin/bash +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi source /opt/ros/melodic/setup.bash source ~/WFH_locobot/ROS/catkin_ws/devel/setup.bash -source ~/WFH_locobot/set_ip.sh $1 $2 \ No newline at end of file diff --git a/locobot/set_wfh_workspace_env.sh b/locobot/set_wfh_workspace_env.sh index 93fb94f..7076186 100644 --- a/locobot/set_wfh_workspace_env.sh +++ b/locobot/set_wfh_workspace_env.sh @@ -3,7 +3,36 @@ # load pyrobot env load_pyrobot_env # source WFH workspace and set_rospkg_path -source ~/WFH_locobot/set_ip.sh $1 $2 +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi + #source ROS/catkin_ws/devel/setup.bash #source ROS/catkin_ws/devel_isolated/setup.bash source ~/WFH_locobot/set_rospackage_path.sh diff --git a/locobot/teleoperation.pyc b/locobot/teleoperation.pyc deleted file mode 100644 index bad2280..0000000 Binary files a/locobot/teleoperation.pyc and /dev/null differ diff --git a/locobot/top_camera.sh b/locobot/top_camera.sh index 8c8d834..ac1988f 100644 --- a/locobot/top_camera.sh +++ b/locobot/top_camera.sh @@ -1,7 +1,36 @@ #!/bin/bash +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi + source ~/WFH_locobot/environment.sh -source ~/WFH_locobot/set_ip.sh $1 $2 rosservice call /calibration #rostopic pub /tilt/command std_msgs/Float64 "data: 0.8" #rostopic pub /pan/command std_msgs/Float64 "data: 0.0" diff --git a/locobot/turn_on_locobot.sh b/locobot/turn_on_locobot.sh index 8f95c3d..051fe7d 100644 --- a/locobot/turn_on_locobot.sh +++ b/locobot/turn_on_locobot.sh @@ -1,4 +1,33 @@ #! /bin/bash +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi + source ~/WFH_locobot/environment.sh -source ~/WFH_locobot/set_ip.sh $1 $2 source ~/WFH_locobot/run_locobot.sh diff --git a/locobot/vr_arm_control.sh b/locobot/vr_arm_control.sh index 1719a57..b6335ae 100644 --- a/locobot/vr_arm_control.sh +++ b/locobot/vr_arm_control.sh @@ -2,7 +2,37 @@ cd ~/WFH_locobot source ~/WFH_locobot/set_wfh_workspace_env.sh -source ~/WFH_locobot/set_ip.sh $1 $2 +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi + + rosrun oculusVR vrarm.py diff --git a/low_cost_ws/README.md b/low_cost_ws/README.md new file mode 100644 index 0000000..b7ced75 --- /dev/null +++ b/low_cost_ws/README.md @@ -0,0 +1,10 @@ +# ROS to Run on Host Laptop/PC + +## Build the Project + +``` +cd ~/low_cost_ws +catkin_make +``` + +## Run diff --git a/low_cost_ws/src/apriltags_ros b/low_cost_ws/src/apriltags_ros new file mode 160000 index 0000000..ff305d0 --- /dev/null +++ b/low_cost_ws/src/apriltags_ros @@ -0,0 +1 @@ +Subproject commit ff305d0c81fba51f2fe5a08a76483a0a7897c1f6 diff --git a/low_cost_ws/src/apriltags_ros_test b/low_cost_ws/src/apriltags_ros_test new file mode 160000 index 0000000..629f635 --- /dev/null +++ b/low_cost_ws/src/apriltags_ros_test @@ -0,0 +1 @@ +Subproject commit 629f63553463a28748b2d907c892af3c29d8172e diff --git a/low_cost_ws/src/arg_utils b/low_cost_ws/src/arg_utils new file mode 160000 index 0000000..6a67903 --- /dev/null +++ b/low_cost_ws/src/arg_utils @@ -0,0 +1 @@ +Subproject commit 6a6790393aab4de623da6afd6e16f7e8828e8bc4 diff --git a/low_cost_ws/src/localization/config/ee632.yaml b/low_cost_ws/src/localization/config/ee632.yaml index b2182e1..352629f 100644 --- a/low_cost_ws/src/localization/config/ee632.yaml +++ b/low_cost_ws/src/localization/config/ee632.yaml @@ -1,36 +1,36 @@ anchor12: id: 0x6a45 - x: 0 - y: 3050 + x: 6500 + y: 6750 z: 950 anchor8: id: 0x6a27 x: 0 - y: 5170 + y: 5020 z: 950 anchor9: id: 0x6a4a - x: 0 - y: 6970 + x: 6500 + y: 4800 z: 950 anchor15: id: 0x6a21 - x: 6500 - y: 2750 + x: 0 + y: 6900 z: 950 anchor14: id: 0x6a42 - x: 6500 - y: 4720 + x: 0 + y: 2920 z: 950 anchor11: id: 0x6a7a x: 6500 - y: 6570 + y: 2700 z: 950 diff --git a/low_cost_ws/src/localization/src/imu_cov.py b/low_cost_ws/src/localization/src/imu_cov.py index e4c00fa..01e46e7 100755 --- a/low_cost_ws/src/localization/src/imu_cov.py +++ b/low_cost_ws/src/localization/src/imu_cov.py @@ -3,13 +3,14 @@ import rospy from sensor_msgs.msg import Imu + def cb(msg): msg.linear_acceleration_covariance = [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] msg.angular_velocity_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.1] + 0.0, 0.0, 0.1] odom_pub.publish(msg) if __name__ == '__main__': diff --git a/low_cost_ws/src/localization/src/localization.py b/low_cost_ws/src/localization/src/localization.py index 18c0791..93d7aa8 100755 --- a/low_cost_ws/src/localization/src/localization.py +++ b/low_cost_ws/src/localization/src/localization.py @@ -1,11 +1,11 @@ #!/usr/bin/env python3 +import time + import rospy from geometry_msgs.msg import PoseWithCovarianceStamped from std_msgs.msg import Float64MultiArray - from uwb import UWB -import time uwb = UWB() @@ -28,6 +28,13 @@ def uwb_error_handler(): def timer_callback(e): now = rospy.Time.now() + validate = uwb.validate() + if not validate: + rospy.logwarn_throttle(1, uwb.network_id_str + " UWB not validated") + uwb.connect() + if uwb.validate(): + rospy.logwarn(uwb.network_id_str + "UWB has reconnected") + uwb.localize_2_5D() uwb.range_all() @@ -35,13 +42,13 @@ def timer_callback(e): distances_pub.publish(distances) - if uwb.pose[0] == 0 and uwb.pose[1] == 0 and uwb.pose[2] == 0: - uwb_error_handler() - return + # if uwb.pose[0] == 0 and uwb.pose[1] == 0 and uwb.pose[2] == 0: + # uwb_error_handler() + # return - if uwb.pose[0] == last_pose[0] and uwb.pose[1] == last_pose[1] and uwb.pose[2] == last_pose[2]: - uwb_error_handler() - return + # if uwb.pose[0] == last_pose[0] and uwb.pose[1] == last_pose[1] and uwb.pose[2] == last_pose[2]: + # uwb_error_handler() + # return pose.header.stamp = now pose.header.frame_id = "map" @@ -60,13 +67,16 @@ def timer_callback(e): if __name__ == "__main__": rospy.init_node("uwb_localization", anonymous=False) - if uwb.connect(): - uwb.reset() - rospy.loginfo("Pozyx UWB connected") - if not uwb.connect(): - uwb.reset() - rospy.loginfo('Connect error') - exit() + + uwb.connect() + + while not uwb.connect() and not rospy.is_shutdown(): + validate = uwb.validate() + if not validate: + rospy.logwarn_throttle(1, uwb.network_id_str + " UWB not validated") + uwb.connect() + if uwb.validate(): + rospy.logwarn(uwb.network_id_str + "UWB has reconnected") uwb.load_env_config(rospy.get_param("~config_file_path")) @@ -74,5 +84,5 @@ def timer_callback(e): uwb.write_env_config() pose_pub = rospy.Publisher("uwb_pose", PoseWithCovarianceStamped, queue_size=10) distances_pub = rospy.Publisher("uwb_distances", Float64MultiArray, queue_size=10) - localization_timer = rospy.Timer(rospy.Duration(0.05), timer_callback) + localization_timer = rospy.Timer(rospy.Duration(0.2), timer_callback) rospy.spin() diff --git a/low_cost_ws/src/localization/src/multilateration_ros.py b/low_cost_ws/src/localization/src/multilateration_ros.py new file mode 100755 index 0000000..b4f6522 --- /dev/null +++ b/low_cost_ws/src/localization/src/multilateration_ros.py @@ -0,0 +1,134 @@ +#! /usr/bin/env python3 + +import time + +import numpy as np +import rospy +import yaml +from geometry_msgs.msg import PoseStamped +from scipy.optimize import minimize +from std_msgs.msg import Float64MultiArray + +method = "Nelder-Mead" + +minimize_options = {} +minimize_options["Nelder-Mead"] = { + "maxiter": int(1e4), + "maxfev": int(1e4), + "xatol": 1e-6, + "fatol": 1e-6, +} +minimize_options["TNC"] = {"maxfun": int(1e3)} +minimize_options["BFGS"] = {"maxiter": int(1e4)} + + +class Args: + def __init__(self, dictionary): + for key, value in dictionary.items(): + setattr(self, key, value) + + +class AnchorLoader: + @staticmethod + def load_yaml(yaml_file): + with open(yaml_file, "r") as file: + try: + data = yaml.safe_load(file) + return data + except yaml.YAMLError as e: + print(f"Error loading YAML file: {e}") + return None + + @staticmethod + def convert_to_numpy(data): + anchor_list = [] + for key, value in data.items(): + anchor_list.append([value["x"], value["y"], value["z"]]) + return np.array(anchor_list) + + @staticmethod + def load_and_convert(yaml_file): + data = AnchorLoader.load_yaml(yaml_file) + if data: + return AnchorLoader.convert_to_numpy(data) + else: + return None + + +class Multilateration: + def __init__(self, args): + self.distances_msg = Float64MultiArray() + self.distances_sub = rospy.Subscriber("uwb1/distances", Float64MultiArray, self.distances_callback) + self.pose_pub = rospy.Publisher("uwb1/pose", PoseStamped, queue_size=100) + self.distances = np.zeros((10, 6)) + self.anchor_pos = AnchorLoader.load_and_convert(args.config_path) + self.robot_pos = np.zeros((1, 3)) + self.optim_pos = np.zeros((1, 3)) + print("Anchor poses:\n", self.anchor_pos) + + def distances_callback(self, msg): + msg.data = np.array(msg.data) + self.distances_msg = msg + self.distances = np.array([msg.data]) + self.optimize() + self.pub_pose() + + def residual(self, robot_pos): + estimated_distances = np.linalg.norm(self.anchor_pos - robot_pos, axis=1) + error = estimated_distances - self.distances + squared_error = np.square(error) + return np.sum(squared_error) + + def optimize(self): + # time this function + start_time = time.time() + result = minimize(self.residual, self.optim_pos, method=method, options=minimize_options[method]) + end_time = time.time() + time_elapsed = end_time - start_time + + if 0 and time_elapsed * 1000.0 > 10.0 or not result.success: + print("Time elapsed: {:.2f} ms".format(time_elapsed * 1000.0)) + print(f"nfev: {result.nfev}") + print(f"nit: {result.nit}") + print(f"succes: {result.success}") + self.optim_pos = np.array([result.x]) + if result.success: + self.robot_pos = np.array([result.x]) + else: + print("Optimization failed to converge.") + print( + "Robot Coordinates (x, y, z): ({:.2f}, {:.2f}, {:.2f})".format( + self.robot_pos[0][0] / 1000.0, + self.robot_pos[0][1] / 1000.0, + self.robot_pos[0][2] / 1000.0, + ) + ) + + def pub_pose(self): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + pose.header.frame_id = "wamv/uwb_origin" + pose.pose.position.x = self.robot_pos[0][0] / 1000.0 + pose.pose.position.y = self.robot_pos[0][1] / 1000.0 + pose.pose.position.z = self.robot_pos[0][2] / 1000.0 + self.pose_pub.publish(pose) + + def update_robot_position(self, new_position): + # Calculate the difference between the new and old positions + diff = new_position - self.robot_pos + norm_diff = np.linalg.norm(diff) + + if norm_diff > 0: + # Scale the difference to stay within the max_position_update_distance + scaling_factor = min(norm_diff, self.max_position_update_distance * 1000) / norm_diff + print("Scaling factor: {:.2f}".format(scaling_factor)) + self.robot_pos += scaling_factor * diff * self.position_update_rate + + +if __name__ == "__main__": + rospy.init_node("multilateration", anonymous=False) + config_path = rospy.get_param("~config_path") + args = {"config_path": config_path} + args = Args(args) + multilateration = Multilateration(args) + rospy.spin() diff --git a/low_cost_ws/src/localization/src/odom_cov.py b/low_cost_ws/src/localization/src/odom_cov.py index fcd72c4..bf61f20 100755 --- a/low_cost_ws/src/localization/src/odom_cov.py +++ b/low_cost_ws/src/localization/src/odom_cov.py @@ -3,13 +3,14 @@ import rospy from nav_msgs.msg import Odometry + def cb(msg): msg.twist.covariance = [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 5.0] + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 5.0] odom_pub.publish(msg) if __name__ == '__main__': diff --git a/low_cost_ws/src/localization/src/ranging.py b/low_cost_ws/src/localization/src/ranging.py new file mode 100755 index 0000000..72368e3 --- /dev/null +++ b/low_cost_ws/src/localization/src/ranging.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 + +import rospy +from std_msgs.msg import Float64MultiArray +from uwb import UWB + +uwb = UWB() + +distances = Float64MultiArray() + + +def timer_callback(e): + # return + now = rospy.Time.now() + validate = uwb.validate() + if not validate: + rospy.logwarn_throttle(1, uwb.network_id_str + " UWB not validated") + uwb.connect() + if uwb.validate(): + rospy.logwarn(uwb.network_id_str + "UWB has reconnected") + + uwb.range_all() + + distances.data = [0.0 for _ in uwb.ranges_all] + + for i in range(len(uwb.ranges_all)): + if uwb.ranges_all[i].distance != 0 and uwb.ranges_all[i].distance < 200000: + distances.data[i] = uwb.ranges_all[i].distance + + distances_pub.publish(distances) + + # print(uwb.network_id, distances.data, validate, end='\r') + + +if __name__ == "__main__": + rospy.init_node("uwb_ranging", anonymous=False) + + config_path = rospy.get_param("~config_path") + port = rospy.get_param("~port", None) + rate = rospy.get_param("~rate", 10) + network_id = rospy.get_param("~network_id", "0x6A1B") + # hex to int + network_id = int(network_id, 16) + uwb.connect(port, network_id) + print(f"uwb.network_id: {uwb.network_id_str}") + uwb.load_env_config(config_path) + # uwb.write_env_config() + + distances_pub = rospy.Publisher("distances", Float64MultiArray, queue_size=10) + localization_timer = rospy.Timer(rospy.Duration(1 / rate), timer_callback) + rospy.spin() diff --git a/low_cost_ws/src/localization/src/uwb.py b/low_cost_ws/src/localization/src/uwb.py index a3bf515..39f149b 100644 --- a/low_cost_ws/src/localization/src/uwb.py +++ b/low_cost_ws/src/localization/src/uwb.py @@ -1,20 +1,21 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../06_uwb.ipynb. - -# %% auto 0 -__all__ = ['UWB'] +from typing import List -# %% ../06_uwb.ipynb 4 -import yaml -import serial -from serial.tools.list_ports import comports import pypozyx -from pypozyx import PozyxSerial -from pypozyx import NetworkID -from pypozyx import Coordinates, DeviceCoordinates -from pypozyx import DeviceRange -from pypozyx import PozyxConstants +import serial +import yaml +from pypozyx import ( + Coordinates, + DeviceCoordinates, + DeviceRange, + NetworkID, + PozyxConstants, + PozyxSerial, +) from pypozyx.core import PozyxException -from typing import List +from pypozyx.definitions.constants import POZYX_SUCCESS +from pypozyx.structures.generic import SingleRegister +from serial.tools.list_ports import comports + # %% ../06_uwb.ipynb 5 class UWB(): @@ -28,10 +29,12 @@ def __init__(self, port = None): self.anchor_ids = [] self._height = 500 + + self._status = 0 @property def network_id(self): - return self._network_id + return self._network_id.id @property def network_id_str(self) -> str: @@ -56,10 +59,6 @@ def network_id(self, value: int = None) -> None: else: self._network_id = NetworkID(value) - -# %% ../06_uwb.ipynb 6 -class UWB(UWB): - # pose @property def pose(self) -> List[float]: """A getter method of UWB pose @@ -83,10 +82,6 @@ def pose(self, value: List[float] = None) -> None: self._pose.y = value[1] self._pose.z = value[2] - -# %% ../06_uwb.ipynb 7 -class UWB(UWB): - # height @property def height(self) -> float: """A getter method of UWB pose height @@ -106,9 +101,6 @@ def height(self, value: float = 0) -> float: self._height = value -# %% ../06_uwb.ipynb 8 -class UWB(UWB): - # env_config @property def env_config(self) -> dict: """A getter method of environment config @@ -119,9 +111,6 @@ def env_config(self) -> dict: return self._env_config -# %% ../06_uwb.ipynb 9 -class UWB(UWB): - # port_lost def port_list(self) -> List[str]: """A getter method of port list. @@ -131,9 +120,6 @@ def port_list(self) -> List[str]: return self._port_list -# %% ../06_uwb.ipynb 10 -class UWB(UWB): - # status @property def status(self) -> int: """A getter method of UWB status. @@ -143,9 +129,6 @@ def status(self) -> int: """ return self._status - -# %% ../06_uwb.ipynb 11 -class UWB(UWB): def load_env_config(self, config_file_path: str) -> bool: """Load UWB anchors' environment config. @@ -160,79 +143,88 @@ def load_env_config(self, config_file_path: str) -> bool: self._env_config = yaml.safe_load(config_file) self.ranges_all = [DeviceRange() for _ in range(len(self._env_config))] self.anchor_ids = [config[1]['id'] for config in self._env_config.items()] - print(self.anchor_ids) + print(hex(self.network_id), [hex(anchor_id) for anchor_id in self.anchor_ids]) except yaml.YAMLError as ex: - print(ex) + # print(ex) return False return True - -# %% ../06_uwb.ipynb 12 -class UWB(UWB): - def scan_port(self) -> None: + def scan_port(self, network_id=None) -> None: """Scan all port connecting to host. Store port device path in port list. """ - self._port_list = [] + if network_id is None: + network_id = self.network_id + + port_list = [] for port in comports(): try: if "Pozyx Labs" in port.manufacturer: - self._port_list.append(port.device) - break + port_list.append(port.device) + continue except TypeError: pass try: if "Pozyx" in port.product: - self._port_list.append(port.device) - break + port_list.append(port.device) + continue except TypeError: pass + print(f'port_list: {port_list}') + if len(port_list) == 1: + return port_list[0] + for port in port_list: + try: + pozyx_handler = PozyxSerial(port) + network_id_ = NetworkID() + pozyx_handler.getNetworkId(network_id_) + print(f'read net: {network_id_}') + print(f'network_id: {hex(network_id)}') + print(f'self network_id: {hex(self.network_id)}') + if network_id == network_id_: + return port + except Exception as e: + # print(e) + pass -# %% ../06_uwb.ipynb 13 -class UWB(UWB): - def reset(self) -> bool: - s = 'F,b0,,1' - if self.port != None: - ser = serial.Serial(self.port) - ser.write(s.encode()) - - def connect(self) -> bool: + def connect(self, port=None, network_id=None) -> bool: """Try to connect pozyx device. Returns: bool: Pozyx status """ self._status = PozyxConstants.STATUS_SUCCESS - if self.port is None: - self.scan_port() - if len(self._port_list) == 1: - self.port = self._port_list[0] - try: - self._pozyx_handler = PozyxSerial(self.port) - self._status &= self._pozyx_handler.getNetworkId(self._network_id) - return self._status - except PozyxException as ex: - print(ex) - self.reset() - return False - elif len(self._port_list) == 0: - print('No Pozyx devices found') - self._status = PozyxConstants.STATUS_FAILURE - return False - else: - return False + + if network_id is not None: + self.network_id = network_id + + if port is not None and network_id is None: + self.port = port else: - try: - self._pozyx_handler = PozyxSerial(self.port) - self._status &= self._pozyx_handler.getNetworkId(self._network_id) - return True - except PozyxException as ex: - print(ex) - self.reset() - return False - + self.port = self.scan_port(network_id) + + print(f'port: {self.port}') + + try: + self._pozyx_handler = PozyxSerial(self.port) + self._status &= self._pozyx_handler.getNetworkId(self._network_id) + return True + except Exception as e: + # print(e) + return False + + + def validate(self, clear_port=False) -> bool: + whoami = SingleRegister() + try: + ret_validate = self._pozyx_handler.getWhoAmI(whoami) == POZYX_SUCCESS + except: + return False + + if clear_port and not ret_validate: + self.port = None + + return ret_validate -# %% ../06_uwb.ipynb 14 -class UWB(UWB): def write_env_config(self) -> bool: """Write environment anchor location into Pozyx UWB device. @@ -250,9 +242,6 @@ def write_env_config(self) -> bool: self._status &= self._pozyx_handler.setSelectionOfAnchorsAutomatic(len(self.env_config)) return self._status - -# %% ../06_uwb.ipynb 15 -class UWB(UWB): def localize_2_5D(self) -> bool: """Localize method in 2.5D. Need to know height. @@ -267,10 +256,7 @@ def localize_2_5D(self) -> bool: ) return self._status - -# %% ../06_uwb.ipynb 16 -class UWB(UWB): - def localize_3D(self)->bool: + def localize_3D(self) -> bool: """Localize method in 3D. The height will be determined by Pozyx UWB device. Returns: @@ -284,9 +270,6 @@ def localize_3D(self)->bool: ) return self._status - -# %% ../06_uwb.ipynb 17 -class UWB(UWB): def range_from(self, dest_id) -> float: """Range method from this Pozyx UWB device to the destination Pozyx UWB device. @@ -300,10 +283,7 @@ def range_from(self, dest_id) -> float: self._status &= self._pozyx_handler.doRanging(dest_id, ranges) return ranges - -# %% ../06_uwb.ipynb 18 -class UWB(UWB): - def range_all(self) -> float: + def range_all(self) -> bool: """Range method from this Pozyx UWB device to the destination Pozyx UWB device. Args: @@ -312,7 +292,9 @@ def range_all(self) -> float: Returns: float: The range from this Pozyx UWB device to the destination Pozyx UWB device. """ - for i, anchor_id in enumerate(self.anchor_ids): - self._status &= self._pozyx_handler.doRanging(anchor_id, self.ranges_all[i]) - - + try: + for i, anchor_id in enumerate(self.anchor_ids): + self._status &= self._pozyx_handler.doRanging(anchor_id, self.ranges_all[i]) + return True + except: + return False diff --git a/low_cost_ws/src/practice_package/CMakeLists.txt b/low_cost_ws/src/practice_package/CMakeLists.txt new file mode 100644 index 0000000..38d4f94 --- /dev/null +++ b/low_cost_ws/src/practice_package/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 3.0.2) +project(practice_package) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES practice_package +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/practice_package.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/practice_package_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_practice_package.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/low_cost_ws/src/practice_package/package.xml b/low_cost_ws/src/practice_package/package.xml new file mode 100644 index 0000000..c16b584 --- /dev/null +++ b/low_cost_ws/src/practice_package/package.xml @@ -0,0 +1,68 @@ + + + practice_package + 0.0.0 + The practice_package package + + + + + rsa + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/low_cost_ws/src/practice_package/src/publisher.py b/low_cost_ws/src/practice_package/src/publisher.py new file mode 100755 index 0000000..57cd268 --- /dev/null +++ b/low_cost_ws/src/practice_package/src/publisher.py @@ -0,0 +1,28 @@ +#!/usr/bin/python3 + +import rospy +from std_msgs.msg import String, Float32 +from geometry_msgs.msg import Twist, PoseStamped + +class Publisher(): + def __init__(self): + self.node_name = rospy.get_name() + rospy.loginfo("[%s] Initializing" % self.node_name) + + #publisher + self.pub_s = rospy.Publisher("/test/", String, queue_size=1) + + self.string = "Hello world!!" + self.rate = rospy.Duration(0.1) + + while not rospy.is_shutdown(): + self.run() + rospy.sleep(self.rate) + + def run(self): + self.pub_s.publish(self.string) + +if __name__=='__main__': + rospy.init_node("Test_Publisher") + publisher = Publisher() + rospy.spin() \ No newline at end of file diff --git a/low_cost_ws/src/practice_package/src/subscriber.py b/low_cost_ws/src/practice_package/src/subscriber.py new file mode 100755 index 0000000..bf83edd --- /dev/null +++ b/low_cost_ws/src/practice_package/src/subscriber.py @@ -0,0 +1,27 @@ +#!/usr/bin/python3 + +import rospy +from std_msgs.msg import String, Float32 +from geometry_msgs.msg import Twist, PoseStamped + +class Subscriber(): + def __init__(self): + self.node_name = rospy.get_name() + rospy.loginfo("[%s] Initializing" % self.node_name) + + #subscriber + self.sub_s = rospy.Subscriber("/test/", String, self.cb_test) + + self.string = "Publisher says : " + self.rate = rospy.Duration(1) + + while not rospy.is_shutdown(): + rospy.sleep(self.rate) + + def cb_test(self, msg): + print(self.string + msg.data + '\n') + +if __name__=='__main__': + rospy.init_node("Test_Subscriber") + subscriber = Subscriber() + rospy.spin() \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/CMakeLists.txt b/low_cost_ws/src/rostest_example/CMakeLists.txt new file mode 100644 index 0000000..372f550 --- /dev/null +++ b/low_cost_ws/src/rostest_example/CMakeLists.txt @@ -0,0 +1,195 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rostest_example) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES rostest_example +# CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(rostest_example +# src/${PROJECT_NAME}/rostest_example.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(rostest_example ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(rostest_example_node src/rostest_example_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(rostest_example_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(rostest_example_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS rostest_example rostest_example_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_rostest_example.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) + +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(tests/quacker_tester.test) + #add_rostest(tests/average_quacks_tester_node.test) + add_rostest(tests/duckiecall_tester_node.test) + #add_rostest(tests/rostest_example_tester_node.test) +endif() diff --git a/low_cost_ws/src/rostest_example/include/for_example/__init__.py b/low_cost_ws/src/rostest_example/include/for_example/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/low_cost_ws/src/rostest_example/include/for_example/import_me_if_u_can.py b/low_cost_ws/src/rostest_example/include/for_example/import_me_if_u_can.py new file mode 100644 index 0000000..d84a991 --- /dev/null +++ b/low_cost_ws/src/rostest_example/include/for_example/import_me_if_u_can.py @@ -0,0 +1,3 @@ +def say_it_works(): + print("You have successed import me!\nfrom for_example pkg :D") + return "It works!" \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py b/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py new file mode 100644 index 0000000..34806cc --- /dev/null +++ b/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 +import numpy as np + +class Quacker(object): + def __init__(self, quack="Quack!"): + self.quack = quack + + def rounded_mean(self, x): + # Returns the mean of x, rounded to the nearest integer + return np.round(np.mean(np.array(x))) + + def get_quack_string(self, n): + # Returns a string of n quacks based on the value in Quacker.quack + return ' '.join([self.quack]*n) diff --git a/low_cost_ws/src/rostest_example/include/rostest_example/__init__.py b/low_cost_ws/src/rostest_example/include/rostest_example/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py b/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py new file mode 100644 index 0000000..6cac729 --- /dev/null +++ b/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py @@ -0,0 +1,17 @@ +import pytest + +from .Quacker import Quacker + +# test get_quack_string function +def test_get_quack_string(): + # initialize a Quacker object + quacker = Quacker() + # test the get_quack_string function + assert quacker.get_quack_string(1) == "Quack!" + +# test rounded mean funcition +def test_rounded_mean(): + # initialize a Quacker object + quacker = Quacker() + # test the rounded_mean function + assert quacker.rounded_mean([1, 2, 3]) == 2 diff --git a/low_cost_ws/src/rostest_example/launch/duckiecall_node.launch b/low_cost_ws/src/rostest_example/launch/duckiecall_node.launch new file mode 100644 index 0000000..5b06f89 --- /dev/null +++ b/low_cost_ws/src/rostest_example/launch/duckiecall_node.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/low_cost_ws/src/rostest_example/package.xml b/low_cost_ws/src/rostest_example/package.xml new file mode 100644 index 0000000..c485e46 --- /dev/null +++ b/low_cost_ws/src/rostest_example/package.xml @@ -0,0 +1,55 @@ + + + rostest_example + 0.0.0 + The rostest_example package + + + + + Teddy Ort + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + roscpp + rospy + + unittest + + + + + + + \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/scripts/add_path.py b/low_cost_ws/src/rostest_example/scripts/add_path.py new file mode 100644 index 0000000..cd0bfd4 --- /dev/null +++ b/low_cost_ws/src/rostest_example/scripts/add_path.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python3 + +import os +import sys +sys.path.append( + os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../include')) +sys.path.append( + os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../../arg_utils/include')) \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py b/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py new file mode 100644 index 0000000..acd6c00 --- /dev/null +++ b/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py @@ -0,0 +1,12 @@ +import pytest + +import add_path +from rostest_example.Quacker import Quacker + +# test Quacker rounded_mean function with a list of integers +def test_quacker_rounded_mean_int(): + q = Quacker() + assert q.rounded_mean([1,2,3,4,5]) == 3 + + + diff --git a/low_cost_ws/src/rostest_example/scripts/test_import_me.py b/low_cost_ws/src/rostest_example/scripts/test_import_me.py new file mode 100644 index 0000000..37fc72d --- /dev/null +++ b/low_cost_ws/src/rostest_example/scripts/test_import_me.py @@ -0,0 +1,7 @@ +import pytest +import add_path + +from for_example.import_me_if_u_can import say_it_works as say_it_pytest + +def test_say_it_from_rostest(): + assert say_it_pytest() == "It works!" diff --git a/low_cost_ws/src/rostest_example/setup.py b/low_cost_ws/src/rostest_example/setup.py new file mode 100644 index 0000000..950ba87 --- /dev/null +++ b/low_cost_ws/src/rostest_example/setup.py @@ -0,0 +1,10 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['rostest_example'], + package_dir={'': 'include'}, +) +setup(**setup_args) \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/src/duckiecall_node.py b/low_cost_ws/src/rostest_example/src/duckiecall_node.py new file mode 100755 index 0000000..f9a744c --- /dev/null +++ b/low_cost_ws/src/rostest_example/src/duckiecall_node.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 +import rospy +from rostest_example.Quacker import * +from std_msgs.msg import String, Int32 + +# Duckiecall Node +# Author: Teddy Ort +# Inputs: ~number_of_quacks/Int32 - The number of quacks that should be in the message +# Outputs: ~duckiecall/String - The output duckiecall message containing a series of quacks + +class DuckiecallNode(object): + def __init__(self): + self.node_name = 'duckiecall_node' + + # Setup the publisher and subscriber + self.sub_num_of_quacks = rospy.Subscriber("~number_of_quacks", Int32, self.quacksCallback) + self.pub_duckiecall = rospy.Publisher("~duckiecall", String, queue_size=1) + + # Setup the quacker + self.quacker = Quacker() + + rospy.loginfo("[%s] has started", self.node_name) + breakpoint() + + def quacksCallback(self, msg_quacks): + msg_duckiecall = String() + msg_duckiecall.data = self.quacker.get_quack_string(msg_quacks.data) + self.pub_duckiecall.publish(msg_duckiecall) + + +if __name__ == '__main__': + rospy.init_node('duckiecall_node', anonymous=False) + duckiecall_node = DuckiecallNode() + rospy.spin() diff --git a/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py new file mode 100755 index 0000000..1de2b73 --- /dev/null +++ b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 +import rospy +import unittest, rostest +from rostest_example.Quacker import * +from std_msgs.msg import String, Int32 + +class DuckiecallTesterNode(unittest.TestCase): + def __init__(self, *args): + super(DuckiecallTesterNode, self).__init__(*args) + self.msg_duckiecall = String() + self.msg_received = False + + def setup(self): + # Setup the node + rospy.init_node('duckiecall_tester_node', anonymous=False) + + # Setup the publisher and subscriber + self.pub_num_of_quacks = rospy.Publisher("~number_of_quacks", Int32, queue_size=1, latch=True) + self.sub_duckiecall = rospy.Subscriber("~duckiecall", String, self.duckiecallCallback) + + # Wait for the node to finish starting up + timeout = rospy.Time.now() + rospy.Duration(5) # Wait at most 5 seconds for the node to come up + while (self.pub_num_of_quacks.get_num_connections() < 1 or self.sub_duckiecall.get_num_connections() < 1) and \ + not rospy.is_shutdown() and rospy.Time.now() < timeout: + rospy.sleep(0.1) + + def duckiecallCallback(self, msg_duckiecall): + self.msg_duckiecall = msg_duckiecall + self.msg_received = True + + def test_publisher_and_subscriber(self): + self.setup() # Setup the node + self.assertGreaterEqual(self.pub_num_of_quacks.get_num_connections(), 1, "No connections found on num_of_quacks topic") + self.assertGreaterEqual(self.sub_duckiecall.get_num_connections(), 1, "No connections found on duckiecall topic") + + def test_duckiecall_output(self): + self.setup() # Setup the node + + # Send the message to the number_of_quacks topic + msg_num_of_quacks = Int32() + msg_num_of_quacks.data = 3 + self.pub_num_of_quacks.publish(msg_num_of_quacks) + rospy.loginfo("Published %d quacks to the number_of_quacks topic", msg_num_of_quacks.data) + + # Wait for the message to be received + timeout = rospy.Time.now() + rospy.Duration(5) # Wait at most 5 seconds for the node to reply + while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout: + rospy.sleep(0.1) + + rospy.loginfo("Received %s from the duckiecall topic", self.msg_duckiecall.data) + + # Send an error if the timeout was hit + self.assertLess(rospy.Time.now(), timeout, "The test timed out with no response from the duckiecall_node") + + # Test the response + response = self.msg_duckiecall.data + self.assertEqual(response, "Quack! Quack! Quack!") # Three Quacks! expected + +if __name__ == '__main__': + rospy.init_node('duckiecall_tester_node', anonymous=False) + rostest.rosrun('rostest_example', 'duckiecall_tester_node', DuckiecallTesterNode) diff --git a/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.test b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.test new file mode 100644 index 0000000..8d67383 --- /dev/null +++ b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/low_cost_ws/src/rostest_example/tests/quacker_tester.py b/low_cost_ws/src/rostest_example/tests/quacker_tester.py new file mode 100755 index 0000000..808da76 --- /dev/null +++ b/low_cost_ws/src/rostest_example/tests/quacker_tester.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +import unittest, rosunit +from rostest_example.Quacker import * + +class QuackerTester(unittest.TestCase): + def test_quacker_default_quack(self): + quacker = Quacker() + self.assertEqual(quacker.quack, "Quack!") + + def test_get_quack_string(self): + quacker = Quacker("Quack!") + msg = quacker.get_quack_string(3) + self.assertEqual(msg, "Quack! Quack! Quack!") + # test with parameter 1 + msg = quacker.get_quack_string(1) + self.assertEqual(msg, "Quack!") + + def test_rounded_average(self): + quacker = Quacker() + x = quacker.rounded_mean([1, 1, 2, 3]) # Average is 7/4 ~ 2 + self.assertEqual(x,2) + +if __name__ == '__main__': + rosunit.unitrun('rostest_example', 'quacker_tester', QuackerTester) diff --git a/low_cost_ws/src/rostest_example/tests/quacker_tester.test b/low_cost_ws/src/rostest_example/tests/quacker_tester.test new file mode 100644 index 0000000..3b08ddd --- /dev/null +++ b/low_cost_ws/src/rostest_example/tests/quacker_tester.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/mit-ocw-lecture-code/README.md b/mit-ocw-lecture-code/README.md new file mode 100644 index 0000000..e2f4297 --- /dev/null +++ b/mit-ocw-lecture-code/README.md @@ -0,0 +1,13 @@ +# Lecture Code from MIT OCW + +* Introduction to Computer Science and Programming in Python +[link](https://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-0001-introduction-to-computer-science-and-programming-in-python-fall-2016/index.htm) + +## Run Pytest + +To run a test_xxx.py file, +``` +pytest -k test_lec8_classes.py +``` +Where -k is to show all print() + diff --git a/mit-ocw-lecture-code/lec8_classes.py b/mit-ocw-lecture-code/lec8_classes.py new file mode 100644 index 0000000..746ebc7 --- /dev/null +++ b/mit-ocw-lecture-code/lec8_classes.py @@ -0,0 +1,123 @@ +################# +## EXAMPLE: simple Coordinate class +################# +class Coordinate(object): + """ A coordinate made up of an x and y value """ + def __init__(self, x, y): + """ Sets the x and y values """ + self.x = x + self.y = y + def __str__(self): + """ Returns a string representation of self """ + return "<" + str(self.x) + "," + str(self.y) + ">" + def distance(self, other): + """ Returns the euclidean distance between two points """ + x_diff_sq = (self.x-other.x)**2 + y_diff_sq = (self.y-other.y)**2 + return (x_diff_sq + y_diff_sq)**0.5 + + +# c = Coordinate(3,4) +# origin = Coordinate(0,0) +# print(c.x, origin.x) +# print(c.distance(origin)) +# print(Coordinate.distance(c, origin)) +# print(origin.distance(c)) +# print(c) + + +################# +## EXAMPLE: simple class to represent fractions +## Try adding more built-in operations like multiply, divide +### Try adding a reduce method to reduce the fraction (use gcd) +################# +class Fraction(object): + """ + A number represented as a fraction + """ + def __init__(self, num, denom): + """ num and denom are integers """ + assert type(num) == int and type(denom) == int, "ints not used" + self.num = num + self.denom = denom + def __str__(self): + """ Retunrs a string representation of self """ + return str(self.num) + "/" + str(self.denom) + def __add__(self, other): + """ Returns a new fraction representing the addition """ + top = self.num*other.denom + self.denom*other.num + bott = self.denom*other.denom + return Fraction(top, bott) + def __sub__(self, other): + """ Returns a new fraction representing the subtraction """ + top = self.num*other.denom - self.denom*other.num + bott = self.denom*other.denom + return Fraction(top, bott) + def __float__(self): + """ Returns a float value of the fraction """ + return self.num/self.denom + def inverse(self): + """ Returns a new fraction representing 1/self """ + return Fraction(self.denom, self.num) + +a = Fraction(1,4) +b = Fraction(3,4) +c = a + b # c is a Fraction object +print(c) +print(float(c)) +print(Fraction.__float__(c)) +print(float(b.inverse())) +##c = Fraction(3.14, 2.7) # assertion error +##print a*b # error, did not define how to multiply two Fraction objects + + +############## +## EXAMPLE: a set of integers as class +############## +class intSet(object): + """ + An intSet is a set of integers + The value is represented by a list of ints, self.vals + Each int in the set occurs in self.vals exactly once + """ + def __init__(self): + """ Create an empty set of integers """ + self.vals = [] + + def insert(self, e): + """ Assumes e is an integer and inserts e into self """ + if not e in self.vals: + self.vals.append(e) + + def member(self, e): + """ Assumes e is an integer + Returns True if e is in self, and False otherwise """ + return e in self.vals + + def remove(self, e): + """ Assumes e is an integer and removes e from self + Raises ValueError if e is not in self """ + try: + self.vals.remove(e) + except: + raise ValueError(str(e) + ' not found') + + def __str__(self): + """ Returns a string representation of self """ + self.vals.sort() + return '{' + ','.join([str(e) for e in self.vals]) + '}' + + +s = intSet() +print(s) +s.insert(3) +s.insert(4) +s.insert(3) +print(s) +s.member(3) +s.member(5) +s.insert(6) +print(s) +#s.remove(3) # leads to an error +print(s) +s.remove(3) diff --git a/mit-ocw-lecture-code/lec9_inheritance.py b/mit-ocw-lecture-code/lec9_inheritance.py new file mode 100644 index 0000000..cafef38 --- /dev/null +++ b/mit-ocw-lecture-code/lec9_inheritance.py @@ -0,0 +1,183 @@ +import random + +################################# +## Animal abstract data type +################################# +class Animal(object): + '''Animal Class + attributes: age + methods: get_age, set_age, get_name, set_name, __str__ + ''' + def __init__(self, age): + self.age = age + self.name = None + def get_age(self): + return self.age + def get_name(self): + return self.name + def set_age(self, newage): + self.age = newage + def set_name(self, newname=""): + self.name = newname + def __str__(self): + return "animal:"+str(self.name)+":"+str(self.age) + +# print("\n---- animal tests ----") +# a = Animal(4) +# print(a) +# print(a.get_age()) +# a.set_name("fluffy") +# print(a) +# a.set_name() +# print(a) + + + +################################# +## Inheritance example +################################# +class Cat(Animal): + ''' + Cat class inherits from Animal class + + Attributes: + name + age + Methods: speak + ''' + def speak(self): + print("meow") + def __str__(self): + return "cat:"+str(self.name)+":"+str(self.age) + +print("\n---- cat tests ----") +c = Cat(5) +c.set_name("fluffy") +print(c) +c.speak() +print(c.get_age()) +#a.speak() # error because there is no speak method for Animal class + + +################################# +## Inheritance example +################################# +class Person(Animal): + def __init__(self, name, age): + Animal.__init__(self, age) + self.set_name(name) + self.friends = [] + def get_friends(self): + return self.friends + def speak(self): + print("hello") + def add_friend(self, fname): + if fname not in self.friends: + self.friends.append(fname) + def age_diff(self, other): + diff = self.age - other.age + print(abs(diff), "year difference") + def __str__(self): + return "person:"+str(self.name)+":"+str(self.age) + + +################################# +## Inheritance example +################################# +class Student(Person): + def __init__(self, name, age, major=None): + Person.__init__(self, name, age) + self.major = major + def __str__(self): + return "student:"+str(self.name)+":"+str(self.age)+":"+str(self.major) + def change_major(self, major): + self.major = major + def speak(self): + r = random.random() + if r < 0.25: + print("i have homework") + elif 0.25 <= r < 0.5: + print("i need sleep") + elif 0.5 <= r < 0.75: + print("i should eat") + else: + print("i am watching tv") + +print("\n---- student tests ----") +s1 = Student('alice', 20, "CS") +s2 = Student('beth', 18) +print(s1) +print(s2) +print(s1.get_name(),"says:", end=" ") +s1.speak() +print(s2.get_name(),"says:", end=" ") +s2.speak() + + + +################################# +## Use of class variables +################################# +class Rabbit(Animal): + # a class variable, tag, shared across all instances + tag = 1 + def __init__(self, age, parent1=None, parent2=None): + Animal.__init__(self, age) + self.parent1 = parent1 + self.parent2 = parent2 + self.rid = Rabbit.tag + Rabbit.tag += 1 + def get_rid(self): + # zfill used to add leading zeroes 001 instead of 1 + return str(self.rid).zfill(3) + def get_parent1(self): + return self.parent1 + def get_parent2(self): + return self.parent2 + def __add__(self, other): + # returning object of same type as this class + return Rabbit(0, self, other) + def __eq__(self, other): + # compare the ids of self and other's parents + # don't care about the order of the parents + # the backslash tells python I want to break up my line + parents_same = self.parent1.rid == other.parent1.rid \ + and self.parent2.rid == other.parent2.rid + parents_opposite = self.parent2.rid == other.parent1.rid \ + and self.parent1.rid == other.parent2.rid + return parents_same or parents_opposite + def __str__(self): + return "rabbit:"+ self.get_rid() + +print("\n---- rabbit tests ----") +print("---- testing creating rabbits ----") +r1 = Rabbit(3) +r2 = Rabbit(4) +r3 = Rabbit(5) +print("r1:", r1) +print("r2:", r2) +print("r3:", r3) +print("r1 parent1:", r1.get_parent1()) +print("r1 parent2:", r1.get_parent2()) + +print("---- testing rabbit addition ----") +r4 = r1+r2 # r1.__add__(r2) +print("r1:", r1) +print("r2:", r2) +print("r4:", r4) +print("r4 parent1:", r4.get_parent1()) +print("r4 parent2:", r4.get_parent2()) + +print("---- testing rabbit equality ----") +r5 = r3+r4 +r6 = r4+r3 +print("r3:", r3) +print("r4:", r4) +print("r5:", r5) +print("r6:", r6) +print("r5 parent1:", r5.get_parent1()) +print("r5 parent2:", r5.get_parent2()) +print("r6 parent1:", r6.get_parent1()) +print("r6 parent2:", r6.get_parent2()) +print("r5 and r6 have same parents?", r5 == r6) +print("r4 and r6 have same parents?", r4 == r6) diff --git a/mit-ocw-lecture-code/test_lec8_classes.py b/mit-ocw-lecture-code/test_lec8_classes.py new file mode 100644 index 0000000..079669f --- /dev/null +++ b/mit-ocw-lecture-code/test_lec8_classes.py @@ -0,0 +1,21 @@ +import lec8_classes as lc + +def test_coordinate(): + c = lc.Coordinate(3, 4) + origin = lc.Coordinate(0,0) + assert c.x == 3 + assert c.y == 4 + assert c.distance(origin) == 5 + assert origin.distance(c) == 5 + +def test_intset(): + s = lc.intSet() + s.insert(3) + s.insert(4) + assert s.member(3) + assert s.member(4) + assert not s.member(5) + s.remove(3) + assert not s.member(3) + assert s.member(4) + diff --git a/mit-ocw-lecture-code/test_lec9_inheritance.py b/mit-ocw-lecture-code/test_lec9_inheritance.py new file mode 100644 index 0000000..f25d635 --- /dev/null +++ b/mit-ocw-lecture-code/test_lec9_inheritance.py @@ -0,0 +1,45 @@ +import lec9_inheritance as inh +import pytest + +def test_animal(): + a = inh.Animal(4) + print(a) + print(a.get_age()) + a.set_name("fluffy") + print(a) + assert a.get_name() == "fluffy" + assert a.get_age() == 4 + +def test_cat(): + c = inh.Cat(5) + print(c) + print(c.get_age()) + c.set_name("fluffy") + print(c) + assert c.get_name() == "fluffy" + assert c.get_age() == 5 + print(c.speak()) + #assert c.speak() == 'meow' + +def test_person(): + p = inh.Person("Captain", 30) + print(p) + print(p.get_age()) + p.set_name("Captain") + print(p) + assert p.get_name() == "Captain" + assert p.get_age() == 30 + + print("\n---- person tests ----") + p1 = inh.Person("jack", 30) + p2 = inh.Person("jill", 25) + print(p1.get_name()) + print(p1.get_age()) + print(p2.get_name()) + print(p2.get_age()) + print(p1) + p1.speak() + p1.age_diff(p2) + + +