-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #7 from FurkanEdizkan/main
Code updates
- Loading branch information
Showing
13 changed files
with
424 additions
and
10 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Binary file modified
BIN
+106 Bytes
(100%)
mobile_manipulator/srvt_moveit/scripts/__pycache__/rokos_smach_lib_realtime.cpython-38.pyc
Binary file not shown.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,174 @@ | ||
| #!/usr/bin/env python3 | ||
|
|
||
| import rospy | ||
| import tf | ||
| import numpy as np | ||
| from tf.transformations import quaternion_from_euler | ||
| from trajectory_msgs.msg import JointTrajectory | ||
| from gazebo_msgs.srv import SpawnModel, DeleteModel | ||
| from geometry_msgs.msg import * | ||
| from visualization_msgs.msg import Marker | ||
| from std_msgs.msg import String | ||
| from nav_msgs.msg import Odometry | ||
| from gazebo_msgs.srv import GetLinkState | ||
| from gazebo_msgs.msg import ModelState | ||
| from gazebo_msgs.srv import SetModelState | ||
| import xml.etree.ElementTree as ET | ||
| import json | ||
| import math | ||
|
|
||
| class DigitalTwinOdt(): | ||
| def __init__(self): | ||
| rospy.init_node("digital_twin_odt") | ||
| self.state_msg = ModelState() | ||
| self.cylinder_chkbx = False | ||
| self.joint_chkbx = False | ||
| self.model_move = rospy.Publisher("/gazebo/set_model_state", ModelState, queue_size = 50) | ||
| rospy.Subscriber("odt_json", String, self.odt_callback) | ||
| rospy.Subscriber("oht_human_info", String, self.oht_callback) | ||
| rospy.Subscriber("mam_chkbx", String, self.mam_chkbx_callback) | ||
| self.pose = Pose() | ||
| self.pose.position.x = 100 | ||
| self.pose.position.y = 100 | ||
| self.pose.position.z = 100 | ||
|
|
||
| rospy.spin() | ||
|
|
||
| def oht_callback(self, msg): | ||
| json_data = json.loads(msg.data) | ||
| self.state_msg.pose.position.x = json_data["x"] | ||
| self.state_msg.pose.position.y = json_data["y"] | ||
| self.state_msg.pose.position.z = 0 | ||
| self.state_msg.model_name = "person_walking" | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| def odt_callback(self, msg): | ||
| pose = Pose() | ||
| data = json.loads(msg.data) | ||
| if self.cylinder_chkbx: | ||
| self.move_cylinder("cylinder1", data["cylinder0"]) | ||
| self.move_cylinder("cylinder2", data["cylinder1"]) | ||
| self.move_cylinder("cylinder4", data["cylinder2"]) | ||
| self.move_cylinder("cylinder5", data["cylinder3"]) | ||
|
|
||
| else: | ||
| self.state_msg.pose = self.pose | ||
| self.state_msg.model_name = "cylinder1" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "cylinder2" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "cylinder4" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "cylinder5" | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| if self.joint_chkbx: | ||
| a = np.array([float(data["dist0"]["fin_pose"]["x"]), float(data["dist0"]["fin_pose"]["y"]) -0.26, float(data["dist0"]["fin_pose"]["z"])]) | ||
| b = np.array([float(data["dist0"]["strt_pose"]["x"]), float(data["dist0"]["strt_pose"]["y"]) -0.26, float(data["dist0"]["strt_pose"]["z"])]) | ||
| orientation = self.calculate_rpy(a, b) | ||
| item_pose = Pose(Point(x=(a[0]+b[0])/2, y=(a[1]+b[1])/2, z=(a[2]+b[2])/2), orientation) | ||
| self.state_msg.model_name = "dist0" | ||
| self.state_msg.pose = item_pose | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| a = np.array([float(data["dist1"]["fin_pose"]["x"]), float(data["dist1"]["fin_pose"]["y"]) -0.26, float(data["dist1"]["fin_pose"]["z"])]) | ||
| b = np.array([float(data["dist1"]["strt_pose"]["x"]), float(data["dist1"]["strt_pose"]["y"]) -0.26, float(data["dist1"]["strt_pose"]["z"])]) | ||
| orientation = self.calculate_rpy(a, b) | ||
| item_pose = Pose(Point(x=(a[0]+b[0])/2, y=(a[1]+b[1])/2, z=(a[2]+b[2])/2), orientation) | ||
| self.state_msg.model_name = "dist1" | ||
| self.state_msg.pose = item_pose | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| a = np.array([float(data["dist2"]["fin_pose"]["x"]), float(data["dist2"]["fin_pose"]["y"]) -0.26, float(data["dist2"]["fin_pose"]["z"])]) | ||
| b = np.array([float(data["dist2"]["strt_pose"]["x"]), float(data["dist2"]["strt_pose"]["y"]) -0.26, float(data["dist2"]["strt_pose"]["z"])]) | ||
| orientation = self.calculate_rpy(a, b) | ||
| item_pose = Pose(Point(x=(a[0]+b[0])/2, y=(a[1]+b[1])/2, z=(a[2]+b[2])/2), orientation) | ||
| self.state_msg.model_name = "dist2" | ||
| self.state_msg.pose = item_pose | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| a = np.array([float(data["dist3"]["fin_pose"]["x"]), float(data["dist3"]["fin_pose"]["y"]) -0.26, float(data["dist3"]["fin_pose"]["z"])]) | ||
| b = np.array([float(data["dist3"]["strt_pose"]["x"]), float(data["dist3"]["strt_pose"]["y"]) -0.26, float(data["dist3"]["strt_pose"]["z"])]) | ||
| orientation = self.calculate_rpy(a, b) | ||
| item_pose = Pose(Point(x=(a[0]+b[0])/2, y=(a[1]+b[1])/2, z=(a[2]+b[2])/2), orientation) | ||
| self.state_msg.model_name = "dist3" | ||
| self.state_msg.pose = item_pose | ||
| self.model_move.publish(self.state_msg) | ||
| else: | ||
| self.state_msg.pose = self.pose | ||
| self.state_msg.model_name = "dist0" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "dist1" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "dist2" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "dist3" | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| def mam_chkbx_callback(self, msg): | ||
| self.cylinder_chkbx = json.loads(msg.data)["gui"]["cylinder"] | ||
| self.joint_chkbx = json.loads(msg.data)["gui"]["joint_dist"] | ||
| if not self.cylinder_chkbx: | ||
| self.state_msg.pose = self.pose | ||
| self.state_msg.model_name = "cylinder1" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "cylinder2" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "cylinder4" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "cylinder5" | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| if not self.joint_chkbx: | ||
| self.state_msg.pose = self.pose | ||
| self.state_msg.model_name = "dist0" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "dist1" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "dist2" | ||
| self.model_move.publish(self.state_msg) | ||
| self.state_msg.model_name = "dist3" | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
|
|
||
| def calculate_rpy(self, vector1, vector2): | ||
| # print(vector1[2], vector2[2]) | ||
| yaw = np.arctan((vector2[1]-vector1[1])/(vector2[0]-vector1[0])) + 1.570796 | ||
| roll = np.arctan((vector2[2]-vector1[2])/(vector2[1]-vector1[1])) + 1.570796 | ||
| pitch = np.arctan((vector2[2]-vector1[2])/(vector2[0]-vector1[0])) + 1.570796 | ||
| if vector1[2] > vector2[2]: | ||
| pitch = np.arctan((vector2[2]-vector1[2])/(vector2[0]-vector1[0])) - 1.570796 | ||
| else: | ||
| pitch = np.arctan((vector2[2]-vector1[2])/(vector2[0]-vector1[0])) + 1.570796 | ||
|
|
||
| quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw) # Quaternion(0,0,0,1) | ||
| orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) | ||
| return orientation | ||
|
|
||
| def move_cylinder(self, name, data): | ||
| pose = Pose() | ||
| pose.position.x = float(data["pose"]["x"]) | ||
| pose.position.y = float(data["pose"]["y"]) - 0.26 | ||
| pose.position.z = float(data["pose"]["z"]) | ||
| if name == "cylinder2": | ||
| pose.position.z += 0.171 | ||
|
|
||
| pose.orientation.x = float(data["pose"]["or_x"]) | ||
| pose.orientation.y = float(data["pose"]["or_y"]) | ||
| pose.orientation.z = float(data["pose"]["or_z"]) | ||
| pose.orientation.w = float(data["pose"]["or_w"]) | ||
| self.state_msg.model_name = name | ||
| self.state_msg.pose = pose | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| def move_dist(self, name, data): | ||
| a = np.array([float(data["dist3"]["fin_pose"]["x"]), float(data["dist3"]["fin_pose"]["y"]) -0.26, float(data["dist3"]["fin_pose"]["z"])]) | ||
| b = np.array([float(data["dist3"]["strt_pose"]["x"]), float(data["dist3"]["strt_pose"]["y"]) -0.26, float(data["dist3"]["strt_pose"]["z"])]) | ||
| orientation = self.calculate_rpy(a, b) | ||
| item_pose = Pose(Point(x=(a[0]+b[0])/2, y=(a[1]+b[1])/2, z=(a[2]+b[2])/2), orientation) | ||
| self.state_msg.model_name = "dist3" | ||
| self.state_msg.pose = item_pose | ||
| self.model_move.publish(self.state_msg) | ||
|
|
||
| if __name__ == '__main__': | ||
| DigitalTwinOdt() |
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,42 @@ | ||
| #!/usr/bin/env python3 | ||
|
|
||
| import numpy as np | ||
| import cv2 | ||
|
|
||
| import rospy | ||
| from sensor_msgs.msg import Image | ||
|
|
||
| class IpCamera2Ros(): | ||
| def __init__(self): | ||
| rospy.init_node("ip_camera_to_ros") | ||
| self.url = "rtsp://admin:[email protected]:554" | ||
| self.fps = 0 | ||
| self.pub = rospy.Publisher("env_cam", Image, queue_size=30) | ||
| self.rate = rospy.Rate(30) | ||
| self.image = Image() | ||
| self.main() | ||
|
|
||
| def main(self): | ||
| cap = cv2.VideoCapture(self.url, cv2.CAP_FFMPEG) | ||
| self.fps = cap.get(cv2.CAP_PROP_FPS) | ||
| if cap.isOpened(): | ||
| while not rospy.is_shutdown(): | ||
| ret, frame = cap.read() | ||
| if ret: | ||
| height, width, channels = frame.shape | ||
| self.image.height = height | ||
| self.image.width = width | ||
| self.image.encoding = "rgb8" | ||
| self.image.header.stamp = rospy.Time.now() | ||
| self.image.header.frame_id = "env_cam" | ||
| cv_rgb_image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) | ||
| self.image.data = bytes(cv_rgb_image) | ||
| self.image.step = width * 1 * channels | ||
| self.pub.publish(self.image) | ||
| # self.image.data = [] | ||
| else: | ||
| break | ||
| self.rate.sleep() | ||
|
|
||
| if __name__ == "__main__": | ||
| IpCamera2Ros() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,41 @@ | ||
| #!/usr/bin/env python3 | ||
|
|
||
| import rospy | ||
| from geometry_msgs.msg import * | ||
| from nav_msgs.msg import Odometry | ||
| from gazebo_msgs.msg import ModelState | ||
| from gazebo_msgs.srv import SetModelState | ||
|
|
||
| class DigitalTwinOdom(): | ||
| def __init__(self): | ||
| rospy.init_node("digital_twin") | ||
| rospy.Subscriber('/odom', Odometry, self.callback) | ||
| rospy.wait_for_service('/gazebo/set_model_state') | ||
| self.set_state = rospy.ServiceProxy( | ||
| '/gazebo/set_model_state', SetModelState) | ||
| self.pre_pose = None | ||
| rospy.spin() | ||
|
|
||
| def callback(self, msg): | ||
| pose_x = msg.pose.pose.position.x | ||
| pose_y = msg.pose.pose.position.y | ||
| pose_orientation = msg.pose.pose.orientation | ||
| # print(pose_x) | ||
| state_msg = ModelState() | ||
| state_msg.model_name = "robot" | ||
| state_msg.pose.position.x = pose_x | ||
| state_msg.pose.position.y = 0 | ||
| state_msg.pose.position.z = 0.162 | ||
| state_msg.pose.orientation.w = 1 | ||
| try: | ||
| if self.pre_pose != None: | ||
| if abs(abs(self.pre_pose) - abs(pose_x)) > 0.01: | ||
| # print(abs(abs(self.pre_pose) - abs(pose_x))) | ||
| self.set_state(state_msg) | ||
| except Exception as e: | ||
| print("fail set product", e) | ||
| self.pre_pose = pose_x | ||
|
|
||
| if __name__ == '__main__': | ||
| DigitalTwinOdom() | ||
|
|
File renamed without changes.
Oops, something went wrong.