Skip to content

Commit

Permalink
finish parallel programming
Browse files Browse the repository at this point in the history
  • Loading branch information
@duISIR
duISIR committed Jun 13, 2023
1 parent b0d5c10 commit 5f74754
Show file tree
Hide file tree
Showing 5 changed files with 1,077 additions and 299 deletions.
2 changes: 1 addition & 1 deletion ...rational_pick_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@
</node>
<!-- Action Service Node for planning joint positions and build MPC to track the planning results -->
<node
name="cmd_pose_server_MPC_BC_operational"
name="planner_server"
pkg="chonk_pushing"
ns="/$(arg robot_name)"
type="action_server_planner_MPC_BC_operational_pick_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn.py"
Expand Down
2 changes: 1 addition & 1 deletion ...rational_pick_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@

<!-- Action Service Node for tracking joint positions and build MPC to track the planning results -->
<node
name="tracking_server_MPC_BC_operational"
name="tracking_server"
pkg="chonk_pushing"
ns="/$(arg robot_name)"
type="action_server_tracking_MPC_BC_operational_pick_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn.py"
Expand Down
179 changes: 112 additions & 67 deletions ..._operational_pick_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,14 @@
import optas
import tf

import threading



class CmdPoseClient(object):
"""docstring for CmdPoseClient."""

def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
def __init__(self, name, client1, client2, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
# initialization message
self._name = name
Expand All @@ -30,71 +33,110 @@ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, t
self._duration = duration
rospy.loginfo("%s: Initialized action client class.", self._name)
# create actionlib client
self._action_client = client
self._action_client1 = client1
self._action_client2 = client2

# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
# wait until actionlib server starts
rospy.loginfo("%s: Waiting for action server to start.", self._name)
self._action_client.wait_for_server()
self._action_client1.wait_for_server()
self._action_client2.wait_for_server()

rospy.loginfo("%s: Action server started, sending goal.", self._name)
# creates goal and sends to the action server
goal = CmdChonkPoseForceGoal()
goal.m_box = self._m_box
goal.poseDonkey.position.x = self._target_pos_Donkey[0]
goal.poseDonkey.position.y = self._target_pos_Donkey[1]
goal.poseDonkey.position.z = self._target_pos_Donkey[2]
goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
goal.poseR.position.x = self._target_pos_R[0]
goal.poseR.position.y = self._target_pos_R[1]
goal.poseR.position.z = self._target_pos_R[2]
goal.poseR.orientation.x = self._target_quat_R[0]
goal.poseR.orientation.y = self._target_quat_R[1]
goal.poseR.orientation.z = self._target_quat_R[2]
goal.poseR.orientation.w = self._target_quat_R[3]
goal.ForceTorqueR.force.x = self._target_force_R[0]
goal.ForceTorqueR.force.y = self._target_force_R[1]
goal.ForceTorqueR.force.z = self._target_force_R[2]
goal.ForceTorqueR.torque.x = self._target_torque_R[0]
goal.ForceTorqueR.torque.y = self._target_torque_R[1]
goal.ForceTorqueR.torque.z = self._target_torque_R[2]
goal.poseL.position.x = self._target_pos_L[0]
goal.poseL.position.y = self._target_pos_L[1]
goal.poseL.position.z = self._target_pos_L[2]
goal.poseL.orientation.x = self._target_quat_L[0]
goal.poseL.orientation.y = self._target_quat_L[1]
goal.poseL.orientation.z = self._target_quat_L[2]
goal.poseL.orientation.w = self._target_quat_L[3]
goal.ForceTorqueL.force.x = self._target_force_L[0]
goal.ForceTorqueL.force.y = self._target_force_L[1]
goal.ForceTorqueL.force.z = self._target_force_L[2]
goal.ForceTorqueL.torque.x = self._target_torque_L[0]
goal.ForceTorqueL.torque.y = self._target_torque_L[1]
goal.ForceTorqueL.torque.z = self._target_torque_L[2]
goal.duration = self._duration
self.goal = CmdChonkPoseForceGoal()
self.goal.m_box = self._m_box
self.goal.poseDonkey.position.x = self._target_pos_Donkey[0]
self.goal.poseDonkey.position.y = self._target_pos_Donkey[1]
self.goal.poseDonkey.position.z = self._target_pos_Donkey[2]
self.goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
self.goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
self.goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
self.goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
self.goal.poseR.position.x = self._target_pos_R[0]
self.goal.poseR.position.y = self._target_pos_R[1]
self.goal.poseR.position.z = self._target_pos_R[2]
self.goal.poseR.orientation.x = self._target_quat_R[0]
self.goal.poseR.orientation.y = self._target_quat_R[1]
self.goal.poseR.orientation.z = self._target_quat_R[2]
self.goal.poseR.orientation.w = self._target_quat_R[3]
self.goal.ForceTorqueR.force.x = self._target_force_R[0]
self.goal.ForceTorqueR.force.y = self._target_force_R[1]
self.goal.ForceTorqueR.force.z = self._target_force_R[2]
self.goal.ForceTorqueR.torque.x = self._target_torque_R[0]
self.goal.ForceTorqueR.torque.y = self._target_torque_R[1]
self.goal.ForceTorqueR.torque.z = self._target_torque_R[2]
self.goal.poseL.position.x = self._target_pos_L[0]
self.goal.poseL.position.y = self._target_pos_L[1]
self.goal.poseL.position.z = self._target_pos_L[2]
self.goal.poseL.orientation.x = self._target_quat_L[0]
self.goal.poseL.orientation.y = self._target_quat_L[1]
self.goal.poseL.orientation.z = self._target_quat_L[2]
self.goal.poseL.orientation.w = self._target_quat_L[3]
self.goal.ForceTorqueL.force.x = self._target_force_L[0]
self.goal.ForceTorqueL.force.y = self._target_force_L[1]
self.goal.ForceTorqueL.force.z = self._target_force_L[2]
self.goal.ForceTorqueL.torque.x = self._target_torque_L[0]
self.goal.ForceTorqueL.torque.y = self._target_torque_L[1]
self.goal.ForceTorqueL.torque.z = self._target_torque_L[2]
self.goal.duration = self._duration
# sends the goal to the action server
rospy.loginfo("%s: Send goal request to action server.", self._name)
self._action_client.send_goal(
goal,
# done_cb=self.done_cb,
# active_cb=self.active_cb,
feedback_cb=self.feedback_cb

# Create threads for sending goals
thread1 = threading.Thread(target=self.action_client1)
thread2 = threading.Thread(target=self.action_client2)
# Start the threads to send goals simultaneously
thread1.start()
thread2.start()

# Wait for both threads to complete
thread1.join()
thread2.join()

def action_client1(self):
self._action_client1.send_goal(
self.goal,
# done_cb=self.done_cb1,
# active_cb=self.active_cb1,
feedback_cb=self.feedback_cb1
)
# wait for the server to finish the action
self._action_client1.wait_for_result()
rospy.loginfo("%s: Got result from action server.", 'client_1')


def action_client2(self):
self._action_client2.send_goal(
self.goal,
# done_cb=self.done_cb2,
# active_cb=self.active_cb2,
feedback_cb=self.feedback_cb2
)
# wait for the server to finish the action
self._action_client.wait_for_result()
rospy.loginfo("%s: Got result from action server.", self._name)
self._action_client2.wait_for_result()
rospy.loginfo("%s: Got result from action server.", 'client_2')

def done_cb1(self, state, result):
rospy.loginfo("%s: Action completed with result %r" % ('client_1', result.reached_goal))
rospy.signal_shutdown("Client request completed.")

def active_cb1(self):
rospy.loginfo("%s: Goal went active!", 'client_1')

def done_cb(self, state, result):
rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
def feedback_cb1(self, feedback):
rospy.loginfo("%s: %.1f%% to completion." % ('client_1', feedback.progress))
pass

def done_cb2(self, state, result):
rospy.loginfo("%s: Action completed with result %r" % ('client_2', result.reached_goal))
rospy.signal_shutdown("Client request completed.")

def active_cb(self):
rospy.loginfo("%s: Goal went active!", self._name)
def active_cb2(self):
rospy.loginfo("%s: Goal went active!", 'client_2')

def feedback_cb(self, feedback):
rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
def feedback_cb2(self, feedback):
rospy.loginfo("%s: %.1f%% to completion." % ('client_2', feedback.progress))
pass

if __name__ == '__main__':
Expand Down Expand Up @@ -185,14 +227,17 @@ def feedback_cb(self, feedback):
parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=8.0)
args = vars(parser.parse_args())

client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
# client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
client1 = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
client2 = actionlib.SimpleActionClient('/chonk/cmd_force', CmdChonkPoseForceAction)


force = 20
m_box = 1.2

# Initialize node class
args['duration']=10
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -219,7 +264,7 @@ def feedback_cb(self, feedback):

# Initialize node class
args['duration']=8.0
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -238,7 +283,7 @@ def feedback_cb(self, feedback):

# Initialize node class
args['duration']=5.0
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -261,7 +306,7 @@ def feedback_cb(self, feedback):

# Initialize node class
args['duration']=5.0
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -283,7 +328,7 @@ def feedback_cb(self, feedback):

# Initialize node class
args['duration']=5.0
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -309,7 +354,7 @@ def feedback_cb(self, feedback):

# # Initialize node class
# args['duration']=4
# cmd_pose_client = CmdPoseClient('client', client,
# cmd_pose_client = CmdPoseClient('client', client1, client2,
# args['m_box'],
# args['target_position_Donkey'],
# args['target_orientation_Donkey'],
Expand All @@ -335,7 +380,7 @@ def feedback_cb(self, feedback):

# Initialize node class
args['duration']=12
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -355,7 +400,7 @@ def feedback_cb(self, feedback):
args['target_position_R'] = [6.18, -0.145, trans_box[2]+0.15]
args['target_position_L'] = [6.18, +0.145, trans_box[2]+0.15]
args['duration']=3
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -376,7 +421,7 @@ def feedback_cb(self, feedback):
args['target_position_L'] = [6.18, +0.145, trans_box[2]]
# Initialize node class
args['duration']=5
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -398,7 +443,7 @@ def feedback_cb(self, feedback):
args['target_position_L'] = [6.18, +0.145, trans_box[2]]
# Initialize node class
args['duration']=5
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -419,7 +464,7 @@ def feedback_cb(self, feedback):
args['target_position_L'] = [6.18, +0.25, trans_box[2]]
# Initialize node class
args['duration']=5
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -440,7 +485,7 @@ def feedback_cb(self, feedback):
args['target_position_L'] = [5, +0.25, trans_box[2]]
# Initialize node class
args['duration']=5
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -466,7 +511,7 @@ def feedback_cb(self, feedback):

# Initialize node class
args['duration']=12.0
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand All @@ -483,7 +528,7 @@ def feedback_cb(self, feedback):

# Initialize node class
args['duration']=2.0
cmd_pose_client = CmdPoseClient('client', client,
cmd_pose_client = CmdPoseClient('client', client1, client2,
args['m_box'],
args['target_position_Donkey'],
args['target_orientation_Donkey'],
Expand Down
Loading

0 comments on commit 5f74754

Please sign in to comment.