6003xuan
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

modify actor control

This commit is contained in:
Your Name 2020-11-12 15:58:46 +08:00
parent e048b84b6c
commit 9e481398bd
8 changed files with 87 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
control/actor/ObstacleAvoid.py
View File

@ -42,7 +42,7 @@ class ObstacleAviod:
else: else:
self.flag = False self.flag = False
subTargList.append(tempTargePos) subTargList.append(tempTargePos)
print subTargList #print subTargList
return subTargList return subTargList
def GetSubTarget(self, startPos, targetPos, safeDis): def GetSubTarget(self, startPos, targetPos, safeDis):
@ -133,10 +133,7 @@ class ObstacleAviod:
elif sign_side == -1: elif sign_side == -1:
angle = minAlpha angle = minAlpha
obstIndex = minAlphaIndex obstIndex = minAlphaIndex
try: resu = self.VectNorm(self.obstList[G[obstIndex]], startPos) / lengthCurToTarget
resu = self.VectNorm(self.obstList[G[obstIndex]], startPos) / lengthCurToTarget
except:
resu =
self.subTarg.y = startPos.y + (curposToTarget.y * math.cos(angle) + math.sin(angle) * curposToTarget.x) * resu self.subTarg.y = startPos.y + (curposToTarget.y * math.cos(angle) + math.sin(angle) * curposToTarget.x) * resu
self.subTarg.x = startPos.x + (curposToTarget.x * math.cos(angle) - math.sin(angle) * curposToTarget.y) * resu self.subTarg.x = startPos.x + (curposToTarget.x * math.cos(angle) - math.sin(angle) * curposToTarget.y) * resu
self.useOriginalCurPos = False self.useOriginalCurPos = False
@ -176,15 +173,15 @@ class ObstacleAviod:
norm = math.sqrt(pow(Pt1.x - Pt2.x, 2) + pow(Pt1.y - Pt2.y, 2)) norm = math.sqrt(pow(Pt1.x - Pt2.x, 2) + pow(Pt1.y - Pt2.y, 2))
return norm return norm
# if __name__ == "__main__": if __name__ == "__main__":
# curr = Point() curr = Point()
# curr.x = float(sys.argv[1]) curr.x = float(sys.argv[1])
# curr.y = float(sys.argv[2]) curr.y = float(sys.argv[2])
# targ = Point() targ = Point()
# targ.x = float(sys.argv[3]) targ.x = float(sys.argv[3])
# targ.y = float(sys.argv[4]) targ.y = float(sys.argv[4])
# avoid = ObstacleAviod() avoid = ObstacleAviod()
# tar = avoid.GetPointList(curr, targ, 1) tar = avoid.GetPointList(curr, targ, 1)

125
control/actor/control_actortest.py
View File

@ -1,6 +1,7 @@
import rospy import rospy
import random import random
from geometry_msgs.msg import Point, Twist from ros_actor_cmd_pose_plugin_msgs.msg import ActorMotion
from geometry_msgs.msg import Point
from gazebo_msgs.srv import GetModelState from gazebo_msgs.srv import GetModelState
import sys import sys
import numpy import numpy
@ -30,8 +31,8 @@ class ControlActor:
self.velocity = 1.5 self.velocity = 1.5
self.last_pose = Point() self.last_pose = Point()
self.current_pose = Point() self.current_pose = Point()
self.target_pose = Point() self.target_motion = ActorMotion()
self.target_pose.z = 1.25 self.target_motion.v = 2
# obstacle avoidance: # obstacle avoidance:
self.Obstacleavoid = ObstacleAviod() #ji wu self.Obstacleavoid = ObstacleAviod() #ji wu
self.avoid_finish_flag = True self.avoid_finish_flag = True
@ -56,7 +57,7 @@ class ControlActor:
[[19.0, 38.5], [-22.0, -6.0]], [[-4.7, 4.4], [-33.4, -21.6]], [[-27.8, -23.5], [-27.5, -15]], [[-36, -24], [-35, -31]], \ [[19.0, 38.5], [-22.0, -6.0]], [[-4.7, 4.4], [-33.4, -21.6]], [[-27.8, -23.5], [-27.5, -15]], [[-36, -24], [-35, -31]], \
[[-35.5, -31.3], [-25.5, -13]], [[3.0,8.2],[34.3,35.7]],[[14,17.5],[-15.7,-12.4]]] [[-35.5, -31.3], [-25.5, -13]], [[3.0,8.2],[34.3,35.7]],[[14,17.5],[-15.7,-12.4]]]
self.box_num = len(self.black_box) self.box_num = len(self.black_box)
self.cmd_pub = rospy.Publisher('/actor_' + str(self.id) + '/cmd_pose', Point, queue_size=10) self.cmd_pub = rospy.Publisher('/actor_' + str(self.id) + '/cmd_motion', ActorMotion, queue_size=10)
#self.black_box = numpy.array( #self.black_box = numpy.array(
# [[[-32, -21], [18, 32]], [[7, 18], [12, 26]], [[55, 66], [15, 29]], [[72, 82], [10, 18]], # [[[-32, -21], [18, 32]], [[7, 18], [12, 26]], [[55, 66], [15, 29]], [[72, 82], [10, 18]],
# [[88, 100], [12, 16]], [[79, 94], [24, 33]], [[54, 69], [-34, -27]], [[-4, 4], [-33, -22]], # [[88, 100], [12, 16]], [[79, 94], [24, 33]], [[54, 69], [-34, -27]], [[-4, 4], [-33, -22]],
@ -100,7 +101,7 @@ class ControlActor:
rate = rospy.Rate(self.f) rate = rospy.Rate(self.f)
while not rospy.is_shutdown(): while not rospy.is_shutdown():
self.target_pose.z = 2.0 self.target_motion.v = 2.0
self.count = self.count + 1 self.count = self.count + 1
# get the pose of uav and actor # get the pose of uav and actor
try: try:
@ -113,8 +114,8 @@ class ControlActor:
self.current_pose.x = 0.0 self.current_pose.x = 0.0
self.current_pose.y = 0.0 self.current_pose.y = 0.0
self.current_pose.z = 1.25 self.current_pose.z = 1.25
distance = (self.current_pose.x - self.target_pose.x) ** 2 + ( distance = (self.current_pose.x - self.target_motion.x) ** 2 + (
self.current_pose.y - self.target_pose.y) ** 2 self.current_pose.y - self.target_motion.y) ** 2
# collosion: if the actor is in the black box, then go backward and update a new target position # collosion: if the actor is in the black box, then go backward and update a new target position
# update new random target position # update new random target position
if (self.avoid_finish_flag and self.distance_flag): if (self.avoid_finish_flag and self.distance_flag):
@ -130,30 +131,30 @@ class ControlActor:
while_time = while_time+1 while_time = while_time+1
print str(self.id) + 'while time : ', while_time print str(self.id) + 'while time : ', while_time
break break
self.target_pose.x = self.x self.target_motion.x = self.x
self.target_pose.y = self.y self.target_motion.y = self.y
self.flag = False self.flag = False
self.distance_flag = False self.distance_flag = False
self.suitable_point = True self.suitable_point = True
self.escape_suce_flag = False self.escape_suce_flag = False
if self.target_pose.x < self.x_min: if self.target_motion.x < self.x_min:
self.target_pose.x = self.x_min self.target_motion.x = self.x_min
elif self.target_pose.x > self.x_max: elif self.target_motion.x > self.x_max:
self.target_pose.x = self.x_max self.target_motion.x = self.x_max
if self.target_pose.y < self.y_min: if self.target_motion.y < self.y_min:
self.target_pose.y = self.y_min self.target_motion.y = self.y_min
elif self.target_pose.y > self.y_max: elif self.target_motion.y > self.y_max:
self.target_pose.y = self.y_max self.target_motion.y = self.y_max
try: try:
print str(self.id)+'general change position' print str(self.id)+'general change position'
self.subtarget_pos = self.Obstacleavoid.GetPointList(self.current_pose, self.target_pose, 1) # current pose, target pose, safe distance self.subtarget_pos = self.Obstacleavoid.GetPointList(self.current_pose, self.target_motion, 1) # current pose, target pose, safe distance
self.subtarget_length = len(self.subtarget_pos) self.subtarget_length = len(self.subtarget_pos)
middd_pos = [Point() for k in range(self.subtarget_length)] middd_pos = [Point() for k in range(self.subtarget_length)]
middd_pos = copy.deepcopy(self.subtarget_pos) middd_pos = copy.deepcopy(self.subtarget_pos)
self.target_pose.x = copy.deepcopy(middd_pos[0].x) self.target_motion.x = copy.deepcopy(middd_pos[0].x)
self.target_pose.y = copy.deepcopy(middd_pos[0].y) self.target_motion.y = copy.deepcopy(middd_pos[0].y)
#self.avoid_start_flag = True #self.avoid_start_flag = True
# print 'current_position: ' + str(self.id)+' ', self.current_pose # print 'current_position: ' + str(self.id)+' ', self.current_pose
# print 'middd_pos: '+ str(self.id)+' ', middd_pos # print 'middd_pos: '+ str(self.id)+' ', middd_pos
@ -170,22 +171,22 @@ class ControlActor:
dis_min = dis_list.index(min(dis_list)) dis_min = dis_list.index(min(dis_list))
self.subtarget_length = 1 self.subtarget_length = 1
if dis_min == 0: if dis_min == 0:
self.target_pose.x = self.black_box[i][0][1] + 1.0 self.target_motion.x = self.black_box[i][0][1] + 1.0
self.target_pose.y = self.current_pose.y self.target_motion.y = self.current_pose.y
elif dis_min == 1: elif dis_min == 1:
self.target_pose.x = self.black_box[i][0][0] - 1.0 self.target_motion.x = self.black_box[i][0][0] - 1.0
self.target_pose.y = self.current_pose.y self.target_motion.y = self.current_pose.y
elif dis_min == 2: elif dis_min == 2:
self.target_pose.y = self.black_box[i][1][1] + 1.0 self.target_motion.y = self.black_box[i][1][1] + 1.0
self.target_pose.x = self.current_pose.x self.target_motion.x = self.current_pose.x
else: else:
self.target_pose.y = self.black_box[i][1][0] - 1.0 self.target_motion.y = self.black_box[i][1][0] - 1.0
self.target_pose.x = self.current_pose.x self.target_motion.x = self.current_pose.x
break break
self.avoid_finish_flag = False self.avoid_finish_flag = False
distance = (self.current_pose.x - self.target_pose.x) ** 2 + ( distance = (self.current_pose.x - self.target_motion.x) ** 2 + (
self.current_pose.y - self.target_pose.y) ** 2 self.current_pose.y - self.target_motion.y) ** 2
if distance < 0.01: if distance < 0.01:
self.arrive_count += 1 self.arrive_count += 1
if self.arrive_count > 5: if self.arrive_count > 5:
@ -235,56 +236,56 @@ class ControlActor:
if (tar_angle < 0) and (flag_k == 0): if (tar_angle < 0) and (flag_k == 0):
y = k*(self.x_max-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y y = k*(self.x_max-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y
if y < self.y_min: if y < self.y_min:
self.target_pose.y = self.y_min self.target_motion.y = self.y_min
self.target_pose.x = (self.y_min - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x self.target_motion.x = (self.y_min - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x
else: else:
self.target_pose.y = y self.target_motion.y = y
self.target_pose.x = self.x_max self.target_motion.x = self.x_max
elif (tar_angle > 0) and (tar_angle < math.pi/2) and (flag_k == 0): elif (tar_angle > 0) and (tar_angle < math.pi/2) and (flag_k == 0):
y = k*(self.x_max-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y y = k*(self.x_max-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y
if y > self.y_max: if y > self.y_max:
self.target_pose.y = self.y_max self.target_motion.y = self.y_max
self.target_pose.x = (self.y_max - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x self.target_motion.x = (self.y_max - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x
else: else:
self.target_pose.y = y self.target_motion.y = y
self.target_pose.x = self.x_max self.target_motion.x = self.x_max
elif (tar_angle < math.pi) and (tar_angle > math.pi/2) and (flag_k == 0): elif (tar_angle < math.pi) and (tar_angle > math.pi/2) and (flag_k == 0):
y = k*(self.x_min-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y y = k*(self.x_min-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y
if y > self.y_max: if y > self.y_max:
self.target_pose.y = self.y_max self.target_motion.y = self.y_max
self.target_pose.x = (self.y_max - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x self.target_motion.x = (self.y_max - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x
else: else:
self.target_pose.y = y self.target_motion.y = y
self.target_pose.x = self.x_min self.target_motion.x = self.x_min
elif (tar_angle > math.pi) and (tar_angle < 3*math.pi/2) and (flag_k == 0): elif (tar_angle > math.pi) and (tar_angle < 3*math.pi/2) and (flag_k == 0):
y = k*(self.x_min-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y y = k*(self.x_min-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y
if y < self.y_min: if y < self.y_min:
self.target_pose.y = self.y_min self.target_motion.y = self.y_min
self.target_pose.x = (self.y_min - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x self.target_motion.x = (self.y_min - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x
else: else:
self.target_pose.y = y self.target_motion.y = y
self.target_pose.x = self.x_min self.target_motion.x = self.x_min
elif flag_k == 1: elif flag_k == 1:
self.target_pose.x = self.gazebo_uav_pose[self.catching_uav_num].x self.target_motion.x = self.gazebo_uav_pose[self.catching_uav_num].x
self.target_pose.y = self.y_max self.target_motion.y = self.y_max
elif flag_k == 2: elif flag_k == 2:
self.target_pose.x = self.gazebo_uav_pose[self.catching_uav_num].x self.target_motion.x = self.gazebo_uav_pose[self.catching_uav_num].x
self.target_pose.y = self.y_min self.target_motion.y = self.y_min
print str(self.id) + ' self.curr_pose:', self.gazebo_uav_pose[self.catching_uav_num] print str(self.id) + ' self.curr_pose:', self.gazebo_uav_pose[self.catching_uav_num]
print str(self.id) + ' self.target_pose:', self.target_pose print str(self.id) + ' self.target_motion:', self.target_motion
print 'escaping change position' print 'escaping change position'
try: try:
print str(self.id)+'general change position' print str(self.id)+'general change position'
self.subtarget_pos = self.Obstacleavoid.GetPointList(self.current_pose, self.target_pose, 1) # current pose, target pose, safe distance self.subtarget_pos = self.Obstacleavoid.GetPointList(self.current_pose, self.target_motion, 1) # current pose, target pose, safe distance
self.subtarget_length = 1 self.subtarget_length = 1
# middd_pos = [Point() for k in range(self.subtarget_length)] # middd_pos = [Point() for k in range(self.subtarget_length)]
# middd_pos = copy.deepcopy(self.subtarget_pos) # middd_pos = copy.deepcopy(self.subtarget_pos)
self.target_pose.x = self.subtarget_pos[0].x self.target_motion.x = self.subtarget_pos[0].x
self.target_pose.y = self.subtarget_pos[0].y self.target_motion.y = self.subtarget_pos[0].y
self.catching_flag = 2 self.catching_flag = 2
except: except:
self.target_pose.x = self.target_pose.x self.target_motion.x = self.target_motion.x
self.target_pose.y = self.target_pose.y self.target_motion.y = self.target_motion.y
self.catching_flag = 1 self.catching_flag = 1
@ -314,16 +315,16 @@ class ControlActor:
self.avoid_finish_flag = True self.avoid_finish_flag = True
self.subtarget_count = 0 self.subtarget_count = 0
else: else:
self.target_pose.x = middd_pos[self.subtarget_count].x self.target_motion.x = middd_pos[self.subtarget_count].x
self.target_pose.y = middd_pos[self.subtarget_count].y self.target_motion.y = middd_pos[self.subtarget_count].y
if self.catching_flag == 1 or self.catching_flag == 2: if self.catching_flag == 1 or self.catching_flag == 2:
self.target_pose.z = 3 self.target_motion.v = 3
else: else:
self.target_pose.z = 2 self.target_motion.v = 2
if self.count % 200 == 0: if self.count % 200 == 0:
print str(self.id) + ' vel:', self.target_pose.z print str(self.id) + ' vel:', self.target_motion.v
self.cmd_pub.publish(self.target_pose) self.cmd_pub.publish(self.target_motion)
rate.sleep() rate.sleep()
def pos2ang(self, deltax, deltay): #([xb,yb] to [xa, ya]) def pos2ang(self, deltax, deltay): #([xb,yb] to [xa, ya])

4
control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/include/actor_plugin_ros/ActorPluginRos.hpp
View File

@ -23,7 +23,7 @@
// Ros // Ros
#include <ros/ros.h> #include <ros/ros.h>
#include <geometry_msgs/Point.h> #include <ros_actor_cmd_pose_plugin_msgs/ActorMotion.h>
#include <ros_actor_cmd_pose_plugin_msgs/ToggleActorWaving.h> #include <ros_actor_cmd_pose_plugin_msgs/ToggleActorWaving.h>
// Ignition // Ignition
@ -66,7 +66,7 @@ namespace gazebo
// private: void NewVelCmdCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); // private: void NewVelCmdCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
/// \brief Callback for newly received pose command /// \brief Callback for newly received pose command
private: void CmdPoseCallback(const geometry_msgs::Point::ConstPtr& cmd_msg); private: void CmdPoseCallback(const ros_actor_cmd_pose_plugin_msgs::ActorMotion::ConstPtr& cmd_msg);
/// \brief Choose New Target /// \brief Choose New Target
private: void ChooseNewTarget(); private: void ChooseNewTarget();

4
control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp
View File

@ -143,12 +143,12 @@ void ActorPluginRos::ChooseNewTarget()
} }
///////////////////////////////////////////////// /////////////////////////////////////////////////
void ActorPluginRos::CmdPoseCallback(const geometry_msgs::Point::ConstPtr &cmd_msg) void ActorPluginRos::CmdPoseCallback(const ros_actor_cmd_pose_plugin_msgs::ActorMotion::ConstPtr &cmd_msg)
{ {
GET_CMD_FLAG = true; GET_CMD_FLAG = true;
target[0] = cmd_msg->x; target[0] = cmd_msg->x;
target[1] = cmd_msg->y; target[1] = cmd_msg->y;
target[2] = cmd_msg->z; target[2] = cmd_msg->v;
//target[2] is cmd_vel //target[2] is cmd_vel
//target = ignition::math::Vector3d(10, 10, 1.0191); //target = ignition::math::Vector3d(10, 10, 1.0191);
//std::cout << "I'm here!" << endl; //std::cout << "I'm here!" << endl;

4
control/actor/run_actor test.sh → control/actor/run_actor_test.sh
View File

@ -1,8 +1,8 @@
#!/bin/bash #!/bin/bash
python control_actortest.py 0 &
python control_actortest.py 1 & python control_actortest.py 1 &
python control_actortest.py 2 & python control_actortest.py 2 &
python control_actortest.py 3 & python control_actortest.py 3 &
python control_actortest.py 4 & python control_actortest.py 4 &
python control_actortest.py 5 & python control_actortest.py 5
python control_actortest.py 0

1
control/actor/stop_control_actors.sh Executable file
View File

@ -0,0 +1 @@
kill -9 $(ps -ef|grep control_actor.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

1
control/actor/stop_set_positions.sh
View File

@ -1 +0,0 @@
kill -9 $(ps -ef|grep set_position.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

12
robocup/score_cal.py
View File

@ -14,16 +14,16 @@ def actor_info_callback(msg):
global target_finish, start_time, time_usage, score, count_flag global target_finish, start_time, time_usage, score, count_flag
actor_id = actor_id_dict[msg.cls] actor_id = actor_id_dict[msg.cls]
for i in actor_id: for i in actor_id:
actor_pos = get_model_state('actor_' + str(actor_id), 'ground_plane').pose.position actor_pos = get_model_state('actor_' + str(i), 'ground_plane').pose.position
if (msg.x-actor_pos.x+coordx_bias)**2+(msg.y-actor_pos.y+coordy_bias)**2<err_threshold**2: if (msg.x-actor_pos.x+coordx_bias)**2+(msg.y-actor_pos.y+coordy_bias)**2<err_threshold**2:
if not count_flag[msg.id]: if not count_flag[i]:
count_flag[msg.id] = True count_flag[i] = True
find_time = rospy.Time.now().secs find_time = rospy.Time.now().secs
elif rospy.Time.now().secs - find_time >= 15: elif rospy.Time.now().secs - find_time >= 15:
target_finish += 1 target_finish += 1
del_model('actor_'+str(msg.id)) del_model('actor_'+str(i))
time_usage = rospy.Time.now().secs - start_time time_usage = rospy.Time.now().secs - start_time
print('actor_'+str(msg.id)+'is OK') print('actor_'+str(i)+'is OK')
print('Time usage:', time_usage) print('Time usage:', time_usage)
# calculate score # calculate score
@ -35,7 +35,7 @@ def actor_info_callback(msg):
score = (1 + target_finish) * 2e2 - sensor_cost * 3e-3 score = (1 + target_finish) * 2e2 - sensor_cost * 3e-3
print('score:',score) print('score:',score)
else: else:
count_flag[msg.id] = False count_flag[i] = False
if __name__ == "__main__": if __name__ == "__main__":
left_actors = range(actor_num) left_actors = range(actor_num)