jasder
/
XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

modify score_cal

This commit is contained in:
Your Name 2020-11-13 00:25:49 +08:00
parent eb7e0edd31
commit 21484e96e8
3 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -78,7 +78,7 @@ void ActorPluginRos::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
// this->velocity =20;
// Make sure the actor stays within bounds
this->init_pose.Pos().X(std::max(-50.0, std::min(100.0, this->init_pose.Pos().X())));
this->init_pose.Pos().X(std::max(-50.0, std::min(150.0, this->init_pose.Pos().X())));
this->init_pose.Pos().Y(std::max(-50.0, std::min(50.0, this->init_pose.Pos().Y())));
this->init_pose.Pos().Z(1.0191);
@ -126,7 +126,7 @@ void ActorPluginRos::ChooseNewTarget()
ignition::math::Vector3d newTarget(this->target);
while ((newTarget - this->target).Length() < 2.0)
{
newTarget.X(ignition::math::Rand::DblUniform(-50, 100));
newTarget.X(ignition::math::Rand::DblUniform(-50, 150));
newTarget.Y(ignition::math::Rand::DblUniform(-50, 50));
for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
@ -256,7 +256,7 @@ void ActorPluginRos::OnUpdate(const common::UpdateInfo &_info)
}
// Make sure the actor stays within bounds
pose.Pos().X(std::max(-50.0, std::min(100.0, pose.Pos().X())));
pose.Pos().X(std::max(-50.0, std::min(150.0, pose.Pos().X())));
pose.Pos().Y(std::max(-50.0, std::min(50.0, pose.Pos().Y())));
pose.Pos().Z(1.0191);

10
robocup/score_cal.py
View File

@ -11,17 +11,17 @@ coordy_bias = 9
actor_id_dict = {'green':[0], 'blue':[1], 'brown':[2], 'white':[3], 'red':[4,5]}
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, left_actors, actors_pos
actor_id = actor_id_dict[msg.cls]
for i in actor_id:
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-actors_pos[i].x+coordx_bias)**2+(msg.y-actors_pos[i].y+coordy_bias)**2<err_threshold**2:
if not count_flag[i]:
count_flag[i] = True
find_time = rospy.Time.now().secs
elif rospy.Time.now().secs - find_time >= 15:
target_finish += 1
del_model('actor_'+str(i))
del left_actors[i]
time_usage = rospy.Time.now().secs - start_time
print('actor_'+str(i)+'is OK')
print('Time usage:', time_usage)
@ -39,6 +39,7 @@ def actor_info_callback(msg):
if __name__ == "__main__":
left_actors = range(actor_num)
actors_pos = [None]*actor_num
count_flag = [False] * actor_num
rospy.init_node('score_cal')
del_model = rospy.ServiceProxy("/gazebo/delete_model",DeleteModel)
@ -70,7 +71,10 @@ if __name__ == "__main__":
start_time = rospy.Time.now().secs
while not rospy.is_shutdown():
for i in left_actors:
actors_pos[i] = get_model_state('actor_' + str(i), 'ground_plane').pose.position
score_pub.publish(score)
left_actors_pub.publish(str(left_actors))
rate.sleep()

13
robocup/test.py Normal file
View File

@ -0,0 +1,13 @@
import rospy
from ros_actor_cmd_pose_plugin_msgs.msg import ActorInfo
if __name__ == "__main__":
rospy.init_node("test")
red_actorinfo = ActorInfo(cls='red',x=10.0,y=10.0)
green_actorinfo = ActorInfo(cls='green',x=10.0,y=10.0)
test_pub_red2 = rospy.Publisher("/actor_red2_info",ActorInfo,queue_size=2)
test_pub_green = rospy.Publisher("/actor_green_info",ActorInfo,queue_size=2)
rospy.Rate(1)
while not rospy.is_shutdown():
test_pub_red2.publish(red_actorinfo)
test_pub_green.publish(green_actorinfo)