computed all ur5_1 trajectories and start of task5

pkg_ros_iot_bridge/config/config_pyiot.yaml

@@ -5,11 +5,11 @@ config_pyiot:
     server_url: "broker.mqttdashboard.com"  # http://www.hivemq.com/demos/websocket-client/
     # server_url: "test.mosquitto.org"   # Alternative to HiveMQ
     server_port: 1883
-    topic_sub: "eyrc/EsNEciqV/iot_to_ros"  # <unique_id> = EsNEciqV
-    topic_pub: "eyrc/EsNEciqV/ros_to_iot"  # <unique_id> = EsNEciqV
+    topic_sub: "/eyrc/EsNEciqV/orders"  # <unique_id> = EsNEciqV
+    topic_pub: "/eyrc/EsNEciqV/ros_to_iot"  # <unique_id> = EsNEciqV
     qos: 1
 
     sub_cb_ros_topic: "/ros_iot_bridge/mqtt/sub"   # ROS nodes can listen to this topic to receive data from MQTT
 
   google_apps:
-    spread_sheet_id: "AKfycbxTpz8LYdWM_tzrNQx2WMPgCpnTEpDb5MVozIiNf9xjJpeboTSt" #"AKfycbw850dk4moVgebU2GGe0PUQUvvg8jTpSjBQCawJt3_13vgujLk" # Spreadsheet Id
+    spread_sheet_id: "AKfycbzcQiV-WQrNW7FbpTFCPyASuCy3c_UPKQiFQ8GQm0byur08x1WvxoZ3Lw" #"AKfycbw850dk4moVgebU2GGe0PUQUvvg8jTpSjBQCawJt3_13vgujLk" # Spreadsheet Id

pkg_ros_iot_bridge/scripts/node_action_server_ros_iot_bridge.py

@@ -14,7 +14,7 @@
 from pkg_ros_iot_bridge.msg import msgMqttSub  # Message Class for MQTT Subscription Messages
 
 from pyiot import iot  # Custom Python Module to perfrom MQTT Tasks
-
+from ast import literal_eval
 
 class ActionServerRosIoTBridge:
 
@@ -184,9 +184,7 @@ def process_goal(self, goal_handle):
 				# update_spreadsheet(spreadsheet_id, id, 'team_id', 'unique_id', '{'turtle_x': x, 'turtle_y': y, 'turtle_theta': theta,...,..}')
 				ret = iot.update_spreadsheet(goal.topic, # using topic to send spreadsheet_id 
 											  self._config_sheet_name,
-											  'VB_1823',  # hardcoded team id ;)
-											  'EsNEciqV',  # hardcoded unique id ;)
-											  goal.message # data points with column names in dict format str
+											  literal_eval(goal.message) # data points with column names in dict format str
 											)
 
 				if ret == 0:

pkg_ros_iot_bridge/scripts/pyiot/iot.py

@@ -6,12 +6,10 @@
 
 """
 
-from ast import literal_eval
 import paho.mqtt.client as mqtt
 import requests
 
 
-
 def on_connect(client, userdata, flags, rc):
 	print("Connected With Result Code "+str(rc))
 
@@ -39,17 +37,16 @@ def mqtt_publish(broker_url, broker_port, pub_topic, payload, qos):
 	return pub_info[0]
 
 
-def update_spreadsheet(spread_sheet_id, sheet_name, team_id, unique_id, data_points_dict_str):
+def update_spreadsheet(spread_sheet_id, sheet_name, data_points_dict):
 	'''
 		This function makes a GET request to any webapp of given "spread_sheet_id" with any number of datapoints 
-		given through "data_points_dict_str" string
-		ex: data_points_dict_str = "{ 'turtle_x': 10, 'turtle_y': 10, 'turtle_theta': 2}" pushes 10 to turtle_x and turtle_y column
+		given through "data_points_dict"
+		ex: data_points_dict = { 'turtle_x': 10, 'turtle_y': 10, 'turtle_theta': 2} pushes 10 to turtle_x and turtle_y column
 		and 2 to turtle_theta column in this format there could be any number of data points
 	'''
-	parameters = {'id':sheet_name, 'team_id':team_id, 'unique_id':unique_id}
+	parameters = {'id':sheet_name, 'Team Id':'VB#1823', 'Unique Id':'EsNEciqV'}
 	url = "https://script.google.com/macros/s/" + spread_sheet_id + "/exec"
 
-	data_points_dict = literal_eval(data_points_dict_str) # converts dict inside the string to dict
 	parameters.update(data_points_dict) # appends datapoints to parameters
 
 	response = requests.get(url, params=parameters)

pkg_task4/launch/task4_solution.launch

@@ -4,7 +4,7 @@
   <arg name="rec_name" default="t4.bag"/>
 
   <!-- Launch task4 simulation in Gazebo --> 
-  <include file="$(find pkg_vb_sim)/launch/task4_simulation.launch" />
+  <include file="$(find pkg_vb_sim)/launch/task5_simulation.launch" />
   <!-- ************************************************************* -->
 
   <!-- Launch two move_group nodes for the two UR5 Arms -->

pkg_task4/scripts/generate_yaml.py

@@ -40,7 +40,7 @@ def __init__(self, arg_robot_name):
 		self._eef_link = self._group.get_end_effector_link()
 		self._group_names = self._robot.get_group_names()
 		self._box_name = ''
-		self._group.set_planning_time(50)
+		self._group.set_planning_time(99)
 		# Attribute to store computed trajectory by the planner	
 		self._computed_plan = ''
 		# Current State of the Robot is needed to add box to planning scene
@@ -360,26 +360,34 @@ def main():
 	'packagen20_angles' : [-0.9605764672406538, -1.643735681421231, 2.029592653976728, 2.756541135143788, -2.159385980446242, 0],
 	'packagen21_angles' : [-2.1173231602835703, -2.060962957306092, 2.315577997893504, 2.886384193245158, -1.0024981825093517, 0],
 	'packagen22_angles' : [-2.8152248676466956, -1.6434319637221213, 2.0297073347102135, 2.7559627506021194, -0.3035895625726175, 0],
-	'packagen30_angles' : [-0.9607709954967598, -1.252967133724633, 2.2894529340625276, 2.1039636047946395, -2.158576423925589, 0]
+	#'packagen30_angles' : [-0.9607709954967598, -1.252967133724633, 2.2894529340625276, 2.1039636047946395, -2.158576423925589, 0],
+	'packagen30_angles' : [-0.9608998456418254, -1.6216061371065917, 2.0668353024057193, -0.4462631714471632, 2.158874320501596, 0],
+	#'packagen31_angles' : [2.1176962476554273, -1.0990099412474503, -2.361680348532907, -2.8222422026694742, -1.045240776091826, 0],
+	'packagen31_angles' : [-2.117204781287308, -2.0420540162747205, 2.361323959613836, -0.3181725103932411, 1.0033663519541758, 0],
+	'packagen32_angles' : [0.9605746664472798, -1.520620110567612, -2.0664038248362546, -2.6969543317823748, -2.202824228465423, 0]
 	}
-	a = ['packagen00_angles','packagen01_angles','packagen02_angles','packagen10_angles','packagen11_angles','packagen12_angles','packagen20_angles','packagen21_angles','packagen30_angles']
+	a = ['packagen00_angles','packagen01_angles','packagen02_angles','packagen10_angles','packagen11_angles','packagen12_angles','packagen20_angles', 'packagen21_angles', 'packagen22_angles', 'packagen30_angles', 'packagen31_angles', 'packagen32_angles']
 	redBinAngles = [-1.4021989827315986, -2.419851300424532, -1.6751688763338333, -0.6181097611262976, 1.5708253066354905, 0]
 	greenBinAngles = [-1.7394663777934447, -0.7215250654445988, 1.674853829440881, -2.523859580495726, -1.571644619132984, 0]
 	yellowBinAngles = [-0.1460482615680263, -0.5637933949566794, 1.3095397138479834, -2.315863329865439, -1.5710326593084032, 0]
 
 	vacuum_gripper = rospy.ServiceProxy('/eyrc/vb/ur5/activate_vacuum_gripper/ur5_1', vacuumGripper)
 	convear_belt = rospy.ServiceProxy('/eyrc/vb/conveyor/set_power', conveyorBeltPowerMsg)
-	#convear_belt(90)
+	convear_belt(90)
 
-	ur5._box_name = 'packagen22'
-	ur5.hard_set_joint_angles(home_angles, 20)
-	ur5.moveit_hard_play_planned_path_from_file(ur5._file_path, 'home_to_packagen22.yaml', 5)
-	vacuum_gripper(1)
-	ur5.attach_box()
-	ur5.moveit_hard_play_planned_path_from_file(ur5._file_path, 'packagen22_to_home.yaml', 5)
+	# ur5._box_name = 'packagen30'
+	# ur5.hard_set_joint_angles(home_angles, 20)
+	# ur5.moveit_hard_play_planned_path_from_file(ur5._file_path, 'home_to_{}.yaml'.format(ur5._box_name), 5)
+	# #ur5.hard_set_joint_angles(angles['{}_angles'.format(ur5._box_name)], 100)
+	# vacuum_gripper(1)
+	# ur5.attach_box()
+	# ur5.moveit_hard_play_planned_path_from_file(ur5._file_path, '{}_to_home.yaml'.format(ur5._box_name), 5)
+	# vacuum_gripper(0)
+	# ur5.detach_box()
+	# ur5.remove_box()
 
 	#ur5.go_to_predefined_pose('allZeros')
-	#ur5.go_to_pose(YellowBinPose)
+	#ur5.go_to_pose(BoxPose)
 	# ur5.attach_box()
 	# ur5.moveit_hard_play_planned_path_from_file(ur5._file_path, 'packagen01_to_home.yaml'.format(ur5._box_name), 5)
 	# for i in a:
@@ -413,25 +421,26 @@ def main():
 	# 	x[ur5._computed_plan] = ur5._computed_time
 	# 	print(ur5._computed_time)
 
-	# y = {}
-	# box_name = 'packagen22'
-	# for i in range(20):
-	# 	ur5.hard_set_joint_angles(home_angles, 20)
-	# 	ur5.hard_set_joint_angles(angles['{}_angles'.format(box_name)], 100)
-	# 	y[ur5._computed_plan] = ur5._computed_time
-	# 	print(ur5._computed_time)
+	y = {}
+	box_name = 'packagen32'
+	for i in range(10):
+		ur5.hard_set_joint_angles(home_angles, 20)
+		ur5.hard_set_joint_angles(angles['{}_angles'.format(box_name)], 100)
+		y[ur5._computed_plan] = ur5._computed_time
+		print(ur5._computed_time)
 
-	# file_path = ur5._file_path + 'home_to_{}_new.yaml'.format(box_name)	
-	# plan = sorted(y.items(),key=lambda l: l[1])[0][0]
-	# with open(file_path, 'w') as file_save:
-	# 	yaml.dump(plan, file_save, default_flow_style=True)
+	file_path = ur5._file_path + 'home_to_{}_new.yaml'.format(box_name)	
+	plan = sorted(y.items(),key=lambda l: l[1])[0][0]
+	with open(file_path, 'w') as file_save:
+		yaml.dump(plan, file_save, default_flow_style=True)
 
 
 	# ur5.hard_set_joint_angles(home_angles, 2)
-	# for i in a[8:]:
+	# for i in a[9:10]:
 	# 	ur5._box_name = i[:-7]
 	# 	print('GOING TO {}'.format(ur5._box_name))
 	# 	ur5.moveit_hard_play_planned_path_from_file(ur5._file_path, 'home_to_{}.yaml'.format(ur5._box_name), 5)
+	# 	#ur5.hard_set_joint_angles(angles['{}_angles'.format(ur5._box_name)], 100)
 	# 	vacuum_gripper(1)
 	# 	ur5.attach_box()
 	# 	ur5.hard_set_joint_angles(home_angles, 1000)

vb_simulation_pkgs/README.md

@@ -10,9 +10,24 @@
 
 
 
+## 25 DEC 2020 | v 1.2.2
+
+* Minor bug fixes.
+  * Red Package QR fixed.
+  * Launch file error fixed.
+
+```
+deleted:    pkg_moveit_ur5_1_config/launch/final_two_ur5_moveit_gazebo.launch
+modified:   pkg_moveit_ur5_1_config/launch/ur5_moveit_controller_manager.launch.xml
+deleted:    pkg_moveit_ur5_2_config/launch/final_two_ur5_moveit_gazebo.launch
+modified:   pkg_moveit_ur5_2_config/launch/ur5_moveit_controller_manager.launch.xml
+```
+
+
+
 ## 25 DEC 2020 | v 1.2.1
 
-* Minor bug fixes
+* Minor bug fixes.
 
 ```
 modified:   pkg_moveit_ur5_1_config/launch/moveit_rviz_robot1.launch

vb_simulation_pkgs/example_pkgs/pkg_moveit_examples/scripts/node_eg4_go_to_pose_2.py

@@ -0,0 +1,164 @@
+#! /usr/bin/env python
+
+import rospy
+import sys
+import copy
+import moveit_commander
+import moveit_msgs.msg
+import geometry_msgs.msg
+import actionlib
+import math
+
+from std_srvs.srv import Empty
+
+class Ur5Moveit:
+
+	# Constructor
+	def __init__(self, arg_robot_name):
+
+		rospy.init_node('node_eg3_set_joint_angles', anonymous=True)
+
+		self._robot_ns = '/'  + arg_robot_name
+		self._planning_group = "manipulator"
+
+		self._commander = moveit_commander.roscpp_initialize(sys.argv)
+		self._robot = moveit_commander.RobotCommander(robot_description= self._robot_ns + "/robot_description", ns=self._robot_ns)
+		self._scene = moveit_commander.PlanningSceneInterface(ns=self._robot_ns)
+		self._group = moveit_commander.MoveGroupCommander(self._planning_group, robot_description= self._robot_ns + "/robot_description", ns=self._robot_ns)
+		self._display_trajectory_publisher = rospy.Publisher( self._robot_ns + '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
+		self._exectute_trajectory_client = actionlib.SimpleActionClient( self._robot_ns + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
+		self._exectute_trajectory_client.wait_for_server()
+
+		self._planning_frame = self._group.get_planning_frame()
+		self._eef_link = self._group.get_end_effector_link()
+		self._group_names = self._robot.get_group_names()
+		self._box_name = ''
+
+		# Current State of the Robot is needed to add box to planning scene
+		self._curr_state = self._robot.get_current_state()
+
+		rospy.loginfo(
+			'\033[94m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')
+		rospy.loginfo(
+			'\033[94m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')
+		rospy.loginfo(
+			'\033[94m' + "Group Names: {}".format(self._group_names) + '\033[0m')
+
+		rospy.loginfo('\033[94m' + " >>> Ur5Moveit init done." + '\033[0m')
+
+	def clear_octomap(self):
+		clear_octomap_service_proxy = rospy.ServiceProxy(self._robot_ns + "/clear_octomap", Empty)
+		return clear_octomap_service_proxy()
+
+	def go_to_pose(self, arg_pose):
+
+		pose_values = self._group.get_current_pose().pose
+		# rospy.loginfo('\033[94m' + ">>> Current Pose:" + '\033[0m')
+		# rospy.loginfo(pose_values)
+
+		self._group.set_pose_target(arg_pose)
+		flag_plan = self._group.go(wait=True)  # wait=False for Async Move
+
+		pose_values = self._group.get_current_pose().pose
+		# rospy.loginfo('\033[94m' + ">>> Final Pose:" + '\033[0m')
+		# rospy.loginfo(pose_values)
+
+		list_joint_values = self._group.get_current_joint_values()
+		# rospy.loginfo('\033[94m' + ">>> Final Joint Values:" + '\033[0m')
+		# rospy.loginfo(list_joint_values)
+
+		if (flag_plan == True):
+			pass
+			# rospy.loginfo(
+			#     '\033[94m' + ">>> go_to_pose() Success" + '\033[0m')
+		else:
+			pass
+			# rospy.logerr(
+			#     '\033[94m' + ">>> go_to_pose() Failed. Solution for Pose not Found." + '\033[0m')
+
+		return flag_plan
+
+
+	def hard_go_to_pose(self, arg_pose, arg_max_attempts):
+
+		number_attempts = 0
+		flag_success = False
+
+		while ( (number_attempts <= arg_max_attempts) and  (flag_success is False) ):
+			number_attempts += 1
+			flag_success = self.go_to_pose(arg_pose)
+			rospy.logwarn("attempts: {}".format(number_attempts) )
+			# self.clear_octomap()
+
+
+
+
+
+	# Destructor
+	def __del__(self):
+		moveit_commander.roscpp_shutdown()
+		rospy.loginfo(
+			'\033[94m' + "Object of class Ur5Moveit Deleted." + '\033[0m')
+
+
+def main():
+
+	ur5 = Ur5Moveit(sys.argv[1])
+
+	ur5_pose_1 = geometry_msgs.msg.Pose()
+	ur5_pose_1.position.x = -0.817261772949
+	ur5_pose_1.position.y = -0.109110076352
+	ur5_pose_1.position.z = 0.94446979642
+	ur5_pose_1.orientation.x = -0.999999995957
+	ur5_pose_1.orientation.y = 4.37354574363e-05
+	ur5_pose_1.orientation.z = 7.85715579538e-05
+	ur5_pose_1.orientation.w = 2.12177767514e-09
+
+	ur5_pose_2 = geometry_msgs.msg.Pose()
+	ur5_pose_2.position.x = -0.414925357653
+	ur5_pose_2.position.y = 0.284932768677
+	ur5_pose_2.position.z = 1.78027849967
+	ur5_pose_2.orientation.x = -0.199396929724
+	ur5_pose_2.orientation.y = 1.64394297608e-05
+	ur5_pose_2.orientation.z = 0.979918803013
+	ur5_pose_2.orientation.w = 6.03911583936e-05
+
+	ur5_pose_3 = geometry_msgs.msg.Pose()
+	ur5_pose_3.position.x = 0.061218702528
+	ur5_pose_3.position.y = 0.150917431354
+	ur5_pose_3.position.z = 1.20083763657
+	ur5_pose_3.orientation.x = 0.635613875737
+	ur5_pose_3.orientation.y = 0.77190802743
+	ur5_pose_3.orientation.z = 0.00233308772292
+	ur5_pose_3.orientation.w = 0.0121472162087
+
+	# Pick Pose
+	ur5_pick_pose = geometry_msgs.msg.Pose()
+	ur5_pick_pose.position.x = 0.030
+	ur5_pick_pose.position.y = 0.250
+	ur5_pick_pose.position.z = 1.890
+
+	# Place Pose
+	ur5_place_pose = geometry_msgs.msg.Pose()
+	ur5_place_pose.position.x = -0.770
+	ur5_place_pose.position.y = -0.062
+	ur5_place_pose.position.z = 0.930
+
+	while not rospy.is_shutdown():
+		rospy.logwarn("\n\nPose#1")
+		ur5.hard_go_to_pose(ur5_pick_pose, 50)
+		rospy.sleep(2)
+
+		rospy.logwarn("\n\nPose#2")
+		ur5.hard_go_to_pose(ur5_place_pose, 50)
+		rospy.sleep(2)
+
+		# rospy.logwarn("\n\nPose#3")
+		# ur5.hard_go_to_pose(ur5_pose_3, 50)
+		# rospy.sleep(2)
+
+	del ur5
+
+
+if __name__ == '__main__':
+	main()

vb_simulation_pkgs/pkg_vb_sim/config/config_online_order.yaml

@@ -0,0 +1,17 @@
+# Config file to store intervals at which to place online orders
+online_order_config:
+  mqtt_server_url: "broker.mqttdashboard.com"
+  mqtt_server_port: 1883
+  mqtt_qos: 2
+  mqtt_unique_id: "eyanthraiitb"
+
+online_order_intervals:
+  package_1: 60
+  package_2: 65
+  package_3: 70
+  package_4: 100
+  package_5: 105
+  package_6: 110
+  package_7: 130
+  package_8: 135
+  package_9: 140

vb_simulation_pkgs/pkg_vb_sim/config/config_package_colour.yaml

@@ -0,0 +1,17 @@
+# Config file for colour of the packages
+package_colour:
+  package_00: "red"
+  package_01: "yellow"
+  package_02: "green"
+
+  package_10: "green"
+  package_11: "yellow"
+  package_12: "red"
+
+  package_20: "green"
+  package_21: "red"
+  package_22: "yellow"
+
+  package_30: "yellow"
+  package_31: "green"
+  package_32: "red"