modify score_cal

control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp

@@ -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);
 

robocup/score_cal.py

@@ -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()
 
 

robocup/test.py

@@ -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)