diff --git a/control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp b/control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp index 4bffc94..db71807 100644 --- a/control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp +++ b/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); diff --git a/robocup/score_cal.py b/robocup/score_cal.py index c2074f7..648d83b 100644 --- a/robocup/score_cal.py +++ b/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= 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() diff --git a/robocup/test.py b/robocup/test.py new file mode 100644 index 0000000..17e6520 --- /dev/null +++ b/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) \ No newline at end of file