diff --git a/scripts/launch.sh b/scripts/launch.sh index e4f87bf..0f8a0c4 100755 --- a/scripts/launch.sh +++ b/scripts/launch.sh @@ -29,7 +29,7 @@ echo "server is finished" docker run -it --rm --name client --network net-sim \ --gpus all \ - --cpus=7.0 -m 8192M \ + --cpus=8 -m 8192M \ -e ROS_MASTER_URI=http://ros-master:11311 \ -e DISPLAY=$DISPLAY \ -e QT_X11_NO_MITSHM=1 \ diff --git a/src/rmus_solution/scripts/manipulater.py b/src/rmus_solution/scripts/manipulater.py index 1ee8ac8..f56a4fd 100755 --- a/src/rmus_solution/scripts/manipulater.py +++ b/src/rmus_solution/scripts/manipulater.py @@ -195,7 +195,7 @@ def trimerworkCallback(self, req): cmd_vel = [0.0, 0.0, 0.0] self.sendBaseVel(cmd_vel) current_time2 = rospy.Time.now() - if current_time2.secs - current_time1.secs > 10:#如果微调时间超过20秒 + if current_time2.secs - current_time1.secs > 20:#如果微调时间超过20秒 rospy.logerr("微调时间过长") self.timeout_pub.publish(True) rospy.sleep(2) @@ -242,7 +242,9 @@ def trimerworkCallback(self, req): flag = 0 y_threshold_p = 0.018 y_threshold_n = 0.018 - x_dis_tar = 0.385 + x_dis_tar = 0.351 + flag_r = 0 + theta = 0.10 while not rospy.is_shutdown(): target_marker_pose = self.current_marker_poses @@ -277,12 +279,26 @@ def trimerworkCallback(self, req): y_threshold_n += 0.01 cmd_vel[1] = -0.11 flag = 1 - - cmd_vel[2] = 0 + + if (target_angle - 0.0) > theta: + if flag_r == 0: + flag_r = 1 + theta += 0.02 + cmd_vel[2] = -0.2 + elif (target_angle - 0.0) < -theta: + if flag_r == 0: + flag_r = 1 + theta += 0.02 + cmd_vel[2] = 0.2 + flag_r = 1 + + #cmd_vel[2] = 0 self.sendBaseVel(cmd_vel) if np.abs(target_pos[0] - x_dis_tar) <= 0.02 and ( (target_pos[1] - 0.0) <= y_threshold_p - and (0.0 - target_pos[1]) <= y_threshold_n + and (0.0 - target_pos[1]) <= y_threshold_n + and(target_angle - 0.0) < theta + and(target_angle - 0.0) > -theta ): rospy.loginfo("Trim well in the all dimention, going open loop") self.sendBaseVel([0.0, 0.0, 0.0]) @@ -326,7 +342,9 @@ def trimerworkCallback(self, req): flag = 0 y_threshold_p = 0.018 y_threshold_n = 0.018 - x_dis_tar = 0.345 + x_dis_tar = 0.351 + flag_r = 0 + theta = 0.10 while not rospy.is_shutdown(): target_marker_pose = self.current_marker_poses @@ -361,12 +379,26 @@ def trimerworkCallback(self, req): y_threshold_n += 0.01 cmd_vel[1] = -0.11 flag = 1 - - cmd_vel[2] = 0 + + if (target_angle - 0.0) > theta: + if flag_r == 0: + flag_r = 1 + theta += 0.02 + cmd_vel[2] = -0.2 + elif (target_angle - 0.0) < -theta: + if flag_r == 0: + flag_r = 1 + theta += 0.02 + cmd_vel[2] = 0.2 + flag_r = 1 + + #cmd_vel[2] = 0 self.sendBaseVel(cmd_vel) if np.abs(target_pos[0] - x_dis_tar) <= 0.02 and ( (target_pos[1] - 0.0) <= y_threshold_p and (0.0 - target_pos[1]) <= y_threshold_n + and(target_angle - 0.0) < theta + and(target_angle - 0.0) > -theta ): rospy.loginfo("Trim well in the all dimention, going open loop") self.sendBaseVel([0.0, 0.0, 0.0]) @@ -437,27 +469,27 @@ def pre(self): rospy.loginfo(": now prepare to grip") pose = Pose() pose.position.x = 0.21 - pose.position.y = -0.035 + pose.position.y = -0.036 self.arm_position_pub.publish(pose) def pre2(self): rospy.loginfo(": level 2 place") pose = Pose() pose.position.x = 0.21 - pose.position.y = 0.015 + pose.position.y = 0.014 self.arm_position_pub.publish(pose) def pre3(self): rospy.loginfo(": level 3 place") pose = Pose() - pose.position.x = 0.19 + pose.position.x = 0.21 pose.position.y = 0.065 self.arm_position_pub.publish(pose) def pre4(self): rospy.loginfo(": level 4 place") pose = Pose() - pose.position.x = 0.19 + pose.position.x = 0.21 pose.position.y = 0.115 self.arm_position_pub.publish(pose) diff --git a/src/rmus_solution/scripts/play_game.py b/src/rmus_solution/scripts/play_game.py index dd0f27e..bea82d5 100755 --- a/src/rmus_solution/scripts/play_game.py +++ b/src/rmus_solution/scripts/play_game.py @@ -158,7 +158,7 @@ def update_rest_block(mode): return sum if __name__ == '__main__': rospy.init_node("gamecore_node") - game_begin_time = rospy.Time.now().to_sec() + # game_begin_time = rospy.Time.now().to_sec() while not rospy.is_shutdown(): try: rospy.wait_for_service("/set_navigation_goal", 1.0) @@ -184,6 +184,7 @@ def update_rest_block(mode): rospy.sleep(0.5) rospy.loginfo("Get all rospy sevice!") + game_begin_time = rospy.Time.now().to_sec() navigation = rospy.ServiceProxy("/set_navigation_goal", setgoal) trimer = rospy.ServiceProxy("/let_manipulater_work", graspsignal) img_switch_mode = rospy.ServiceProxy("/image_processor_switch_mode", switch)