FreiCar Agent
This node performs the bare minimum required for running an agent in the FreiCar software stack. This will be your starting point. As mentioned in the overview, this node by default takes care of the following tasks:
- Sending
Track
requests at start and shutdown - Publishing
FreiCarStatus
messages - Publishing
FreiCarLocalization
messages - Publishing a 3D model for visualization on RVIZ
In the following, we'll go through the important parts of the code.
1. FreiCarAgent Class
Instances of this class represent the agents in the environment.
class FreiCarAgent
{
public:
struct Settings {
unsigned int type_code;
int thread_sleep_ms;
std::string name;
std::string owner;
std::string tf_name;
std::string map_path;
};
~FreiCarAgent();
FreiCarAgent(FreiCarAgent::Settings settings, std::shared_ptr<ros::NodeHandle> nh);
void SendTrackRequest(bool track_enable);
void StopAgentThread();
private:
void Step(unsigned int thread_sleep_ms);
void HaltCallback(const freicar_common::FreiCarHalt::ConstPtr &msg);
void ResumeCallback(const std_msgs::String::ConstPtr &msg);
void RGBCallback(const sensor_msgs::ImageConstPtr& msg);
void DepthCallback(const sensor_msgs::ImageConstPtr& msg);
void InitializeMap();
void Initialize3DModel();
void PublishStatus(const ros::Time& now);
void PublishLocalization(const ros::Time& now, const map::Map& map_instance);
// private members
...
};
1.1. Settings struct
This struct contains the parameters that are set via ROS or read from the host:
- type_code: could be
freicar::agent::type::REAL
orfreicar::agent::type::SIMULATED
or both (bitwise or), set via local ROS parameter - thread_sleep_ms: global ROS parameter set by the 'freicar_setting` node
- name: local ROS parameter set by the launch file
- owner: "username@hostname"
- tf_name: name of the tf frames for this agent (same as name for simulated agents)
- map_path: initialize the map from this file if not received over the network
Note that if any of these parameters are missing, the node exits abruptly. E.g. thread_sleep_ms
relies on freicar_setting
for the sim_sps
parameter. If that node is not running, intialization fails.
2. Public Functions
2.1. FreiCarAgent()
In the constructor, the static parts of messages are set, the publishers and subscribers are initialized and the main thread is started. InitializeMap()
initializes the map as the name suggests. The code is very similar to the code snippet explained in FreiCar Map. Intitilization of future member variables should be performed here as well.
2.2. SendTrackRequest()
This function is responsible for sending a Track
request to chaperone. As mentioned in the overview, chaperone is responsible for making sure the agents don't collide. Multiple assumptions are made when sending these requests:
- The chaperone node is running. If not, the track request fails and the node exits.
- The parallel
carla_proxy
node is manually SIGINT'd, if the track request fails for any reason. That way [hopefully] the agent is also removed from the simulator. Although this is not a serious problem, it helps avoid restarting the simulator every time this node fails. - This node is manually SIGINT'd, if the parallel
carla_prxoy
node crashes for any reason so that a stop-track request can be sent.
2.3. StopAgentThread()
This function is automatically called in the destructor. It sets the running
flag to false and joins the thread. In normal circumstances, you don't need to directly call this function.
3. Private Functions
3.1. Step()
This is the main thread callback function for the agent. Its sleep period is set according to the global parameter sim_sps
.
void FreiCarAgent::Step(unsigned int thread_sleep_ms) {
// transform structs
tf2_ros::Buffer transform_buffer;
tf2_ros::TransformListener tf2_listener(transform_buffer);
auto& map_instance = map::Map::GetInstance();
while (running_) {
try {
current_pose_ = transform_buffer.lookupTransform("map", settings_.tf_name, ros::Time(0));
} catch (tf2::TransformException &ex) {
ros::Duration(0.2).sleep();
continue;
}
// your code here
auto now = ros::Time::now();
PublishStatus(now);
PublishLocalization(now, map_instance);
model_pub_.publish(model3d_);
std::this_thread::sleep_for(std::chrono::milliseconds(thread_sleep_ms));
}
}
In each iteration, the current pose is updated via tf.lookupTransform
. The two functions PublishStatus
and PublishLocalization
take care of publishing necessary messages for each agent. The subsequent call also publishes a basic 3D model for visualization on RVIZ. Naturally this is not a necessary task and can be commented out.
3.2. Halt and Resume Callbacks
These messages are usually sent from the chaperone node. When a Halt
message is received, the agent is expected to stop in the shortest distance possible. Currently, we only keep track of the suspensions using a flag, a string and an enum to indicate the state, the other agent involved in the suspension event and the type of the suspension respectively. The different types of suspension can be looked up in the HaltType
enum. When the suspension is lifted, the agent is allowed to resume its activity.
3.3. RGBCallback
This is the callback function for RGB images. Currently, they are published in the carla_proxy
node.
void FreiCarAgent::RGBCallback(const sensor_msgs::ImageConstPtr& msg) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat rgb8_img = cv_ptr->image;
// your code here
}
The image is cast to an RGB8 cv::Mat
. At this point you can use the image for any downstream task.
3.4. DepthCallback
void FreiCarAgent::DepthCallback(const sensor_msgs::ImageConstPtr& msg) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat float32_img = cv_ptr->image;
// your code here
}
The image is cast to a float32 cv::Mat
. Each pixel in the depth image corresponds to the same pixel in the RGB image. You should therefor keep track of the received images (e.g. using an std::vector<>
member variable) if you want to process RGBD data.
Note
We highly advise you to familiarize yourself with memory management in C++. OpenCV data structures use a scheme that is very similar to the standard shared pointer and they should be treated as such.