Here is my XML document:
<?xml version="1.0" ?>
<argos-configuration>
<!-- General configuration -->
<framework>
<system threads="0" />
<experiment length="0" ticks_per_second="10" random_seed="124" />
</framework>
<!-- Controllers -->
<controllers>
<footbot_trajectoryplaning_controller id="ftp" library="/home/baoqinghan/argos_projects/footbot_trajectory_planing/build/controllers/libfootbot_trajectoryplaning_controller.so">
<actuators>
<differential_steering implementation="default" />
<gripper implementation="default" />
</actuators>
<sensors>
<footbot_proximity implementation="default" show_rays="true" />
<positioning implementation="default" />
</sensors>
<params alpha="7.5" delta="0.1" velocity="5" />
</footbot_trajectoryplaning_controller>
</controllers>
<!-- Loop functions -->
<loop_functions library="/home/baoqinghan/argos_projects/footbot_trajectory_planing/build/loop_functions/libtrajectory_loop_functions.so" label="trajectory_loop_functions" />
<!-- Arena configuration -->
<arena size="5, 5, 1" center="0,0,0.5">
<box id="wall_north" size="4,0.1,0.5" movable="false">
<body position="0,2,0" orientation="0,0,0" />
</box>
<box id="wall_south" size="4,0.1,0.5" movable="false">
<body position="0,-2,0" orientation="0,0,0" />
</box>
<box id="wall_east" size="0.1,4,0.5" movable="false">
<body position="2,0,0" orientation="0,0,0" />
</box>
<box id="wall_west" size="0.1,4,0.5" movable="false">
<body position="-2,0,0" orientation="0,0,0" />
</box>
<!-- Adding two vertical entities for robots to reach -->
<box id="vertical_entity_1" size="2.5,0.1,0.1" movable="false">
<body position="0,1,0" orientation="0,0,0" />
</box>
<box id="vertical_entity_2" size="2.5,0.1,0.1" movable="false">
<body position="0,0,0" orientation="0,0,0" />
</box>
<!-- Adding small objects to the left of each vertical entity -->
<box id="small_object_1" size="0.1,0.1,0.1" movable="false">
<body position="-0.6,1.2,0" orientation="0,0,0" />
</box>
<box id="small_object_4" size="0.1,0.1,0.1" movable="false">
<body position="0.6,-0.2,0" orientation="0,0,0" />
</box>
<!-- Distributing foot-bots -->
<distribute>
<entity quantity="1" max_trials="100">
<foot-bot id="fb0">
<controller config="ftp" />
<position method="constant" values="1,-1,0" />
<orientation method="constant" values="0,0,0" />
</foot-bot>
</entity>
<entity quantity="1" max_trials="100">
<foot-bot id="fb1">
<controller config="ftp" />
<position method="constant" values="2,-1,0" />
<orientation method="constant" values="0,0,0" />
</foot-bot>
</entity>
</distribute>
<!-- Distributing 5 cylinders randomly -->
<distribute>
<position method="uniform" min="-2,-2,0" max="2,2,0" />
<orientation method="constant" values="0,0,0" />
<entity quantity="5" max_trials="100">
<cylinder id="c" height="0.5" radius="0.15" movable="false" />
</entity>
</distribute>
</arena>
<!-- Physics engines -->
<physics_engines>
<dynamics2d id="dyn2d" />
</physics_engines>
<!-- Media -->
<media />
<!-- Visualization -->
<visualization>
<qt-opengl>
<user_functions library="/home/baoqinghan/argos_projects/footbot_trajectory_planing/build/loop_functions/libtrajectory_loop_functions.so" label="trajectory_qtuser_functions" />
<camera>
<placements>
<placement index="0" position="-6.83677,-0.0492575,7.1301" look_at="-6.14438,-0.0492575,6.40857" up="0.721522,-3.4075e-17,0.692391" lens_focal_length="65" />
</placements>
</camera>
</qt-opengl>
</visualization>
</argos-configuration>
Here are faults:
[FATAL] Failed to initialize the space.
[FATAL] Error while trying to distribute entities
[FATAL] Node 'position' not found
Here is my controller document:
#include <argos3/core/control_interface/ci_controller.h>
#include <argos3/plugins/robots/foot-bot/simulator/footbot_entity.h>
#include <argos3/core/simulator/simulator.h>
#include <argos3/core/simulator/entity/embodied_entity.h>
#include <argos3/core/utility/logging/argos_log.h>
#include <argos3/core/utility/math/vector2.h>
#include <argos3/core/utility/math/rng.h>
#include <argos3/plugins/robots/foot-bot/control_interface/ci_footbot_motor_ground_sensor.h>
#include <argos3/plugins/robots/foot-bot/control_interface/ci_footbot_proximity_sensor.h>
#include <argos3/plugins/robots/generic/control_interface/ci_differential_steering_actuator.h>
using namespace argos;
class CFootBotTrajectoryplaningController : public CCI_Controller {
public:
CFootBotTrajectoryplaningController() :
m_pcWheels(nullptr),
m_pcProximitySensor(nullptr),
m_cTargetPosition(-0.6,1.2), // 目标位置
m_fWheelVelocity(2.5f) {}
virtual ~CFootBotTrajectoryplaningController() {}
virtual void Init(TConfigurationNode& t_node) {
m_pcWheels = GetActuator<CCI_DifferentialSteeringActuator>("differential_steering");
m_pcProximitySensor = GetSensor<CCI_FootBotProximitySensor>("footbot_proximity");
// 获取仿真空间和实体
try {
CFootBotEntity& cFootBotEntity = dynamic_cast<CFootBotEntity&>(CSimulator::GetInstance().GetSpace().GetEntity(GetId()));
CVector3 cPosition3D = cFootBotEntity.GetEmbodiedEntity().GetOriginAnchor().Position;
m_cCurrentPosition.Set(cPosition3D.GetX(), cPosition3D.GetY());
} catch (const std::exception& ex) {
THROW_ARGOSEXCEPTION("Error retrieving FootBot entity: " << ex.what());
}
}
virtual void ControlStep() {
// 使用实际位置
CVector2 cCurrentPos = m_cCurrentPosition;
// 计算到目标的向量
CVector2 cToTarget = m_cTargetPosition - cCurrentPos;
cToTarget.Normalize();
// 获取传感器读数并计算避障所需的转向
const CCI_FootBotProximitySensor::TReadings& tProxReads = m_pcProximitySensor->GetReadings();
CVector2 cAvoidanceVector(0,0);
for(const auto& tRead : tProxReads) {
CVector2 cTemp;
cTemp.FromPolarCoordinates(1.0f / tRead.Value, tRead.Angle);
if(tRead.Value > 0) {
cAvoidanceVector -= cTemp;
}
}
if(cAvoidanceVector.Length() > 0.0f) {
cAvoidanceVector.Normalize();
cToTarget += cAvoidanceVector;
cToTarget.Normalize();
}
// 根据合成向量设置轮速
CRadians cAngle = cToTarget.Angle().SignedNormalize();
Real fLeftSpeed = m_fWheelVelocity;
Real fRightSpeed = m_fWheelVelocity;
if(cAngle > CRadians::ZERO) {
fRightSpeed *= 1.0 - Min(cAngle / CRadians::PI, 1.0);
} else {
fLeftSpeed *= 1.0 - Min(-cAngle / CRadians::PI, 1.0);
}
m_pcWheels->SetLinearVelocity(fLeftSpeed, fRightSpeed);
}
virtual void Reset() {
m_pcWheels->SetLinearVelocity(0.0f, 0.0f);
}
virtual void Destroy() {}
private:
CCI_DifferentialSteeringActuator* m_pcWheels;
CCI_FootBotProximitySensor* m_pcProximitySensor;
CVector2 m_cCurrentPosition; // 实时更新的当前位置
CVector2 m_cTargetPosition; // 目标位置
Real m_fWheelVelocity;
};
REGISTER_CONTROLLER(CFootBotTrajectoryplaningController, "footbot_trajectoryplaning_controller")
And when I change my distribute to:
<distribute>
<position method="constant" values="1,-1,0" />
<orientation method="constant" values="0,0,0" />
<entity quantity="1" max_trials="100">
<foot-bot id="fb0">
<controller config="ftp" />
</foot-bot>
</entity>
</distribute>
The fault will be:
[FATAL] Failed to initialize the space.
[FATAL] Error while trying to distribute entities
[FATAL] Failed to initialize entity "fb00".
[FATAL] Failed to initialize controllable entity "controller_0".
[FATAL] Can't set controller for controllable entity "controller_0"
[FATAL] Error retrieving FootBot entity: [FATAL] Unknown entity id "fb00" when requesting entity from space.
I don't understand the reason. Could you help me?