Skip to content

Commit

Permalink
Merge pull request #17 from lge-ros2/main
Browse files Browse the repository at this point in the history 
Merge 'main' branch into 'foxy'
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Feb 3, 2021
2 parents 58fe767 + ccb637f commit 74a06d3
Show file tree
Hide file tree
Showing 5 changed files with 145 additions and 108 deletions.
18 changes: 16 additions & 2 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,12 +94,26 @@ docker build -t cloisim_ros .
docker run -it --rm --net=host -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID cloisim_ros

docker run -it --rm --net=host -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID cloisim_ros singlemode:=False

docker run -it --rm --net=host -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID cloisim_ros singlemode:=True
```

or
with default network driver(bridge)

```shell
docker run -it --rm --net=host -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID cloisim_ros singlemode:=True
docker run -it --rm -e CLOISIM_BRIDGE_IP='192.168.0.125' -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID cloisim_ros

docker run -it --rm -e CLOISIM_BRIDGE_IP='192.168.0.125' -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID cloisim_ros singlemode:=False

docker run -it --rm -e CLOISIM_BRIDGE_IP='192.168.0.125' -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID cloisim_ros singlemode:=True
```

You can set bridge ip using below command.

```shell
echo $(ip addr show dev docker0 | grep "inet" | awk 'NR==1{print $2}' | cut -d'/' -f 1)

docker run -it --rm -e CLOISIM_BRIDGE_IP=`echo $(ip addr show dev docker0 | grep "inet" | awk 'NR==1{print $2}' | cut -d'/' -f 1)` -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID cloisim_ros
```

## Version info
Expand Down
4 changes: 2 additions & 2 deletions cloisim_ros_base/src/base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,10 +162,10 @@ void Base::CloseBridges()
string Base::GetRobotName()
{
bool isSingleMode;
get_parameter("single_mode", isSingleMode);
get_parameter("singlemode", isSingleMode);

string robotName;
get_parameter("single_mode.robotname", robotName);
get_parameter("singlemode.robotname", robotName);

return (isSingleMode) ? robotName : string(get_namespace()).substr(1);
}
Expand Down
2 changes: 1 addition & 1 deletion cloisim_ros_bringup/include/cloisim_ros_bringup/bringup_param.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ class BringUpParam : public rclcpp::Node
{
get_parameter("singlemode", isSingleMode);

RCLCPP_INFO_ONCE(this->get_logger(), "singleMode: %d", isSingleMode);
RCLCPP_INFO_ONCE(this->get_logger(), "single mode: %d", isSingleMode);
};

bool IsSingleMode() const { return isSingleMode; }
Expand Down
185 changes: 96 additions & 89 deletions cloisim_ros_bringup/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,142 +64,149 @@ int main(int argc, char** argv)
const auto isSingleMode = bringup_param_node->IsSingleMode();

rclcpp::NodeOptions default_node_options;
default_node_options.append_parameter_override("single_mode", bool(isSingleMode));
default_node_options.append_parameter_override("singlemode", bool(isSingleMode));

std::vector<std::shared_ptr<cloisim_ros::Base>> node_list;
std::vector<std::shared_ptr<cloisim_ros::Base>> rclcpp_node_list;

for (auto it = result_map.begin(); it != result_map.end(); it++)
{
const auto item_name = it.key().asString();
const auto item_list = (*it);

cout << "# Item Name: " << item_name << endl;
cout << "Item Name: " << item_name << endl;

for (auto it2 = item_list.begin(); it2 != item_list.end(); ++it2)
{
const auto node_type = it2.key().asString();
const auto node_name = (*it2).begin().key().asString();
const auto item = (*it2)[node_name];
const auto node_list = (*it2);

cout << "Node Type: " << node_type << ", Name: " << node_name << endl;
// cout << item << endl;
cout << "\tNode Type: " << node_type << endl;

std::shared_ptr<cloisim_ros::Base> node = nullptr;
rclcpp::NodeOptions node_options(default_node_options);

if (isSingleMode)
for (auto it3 = node_list.begin(); it3 != node_list.end(); ++it3)
{
node_options.append_parameter_override("single_mode.robotname", item_name);
}
const auto node_name = it3.key().asString();
const auto item = (*it2)[node_name];

// store parameters
for (auto paramIt = item.begin(); paramIt != item.end(); ++paramIt)
{
const auto bridge_key = paramIt.key().asString();
const auto bridge_port = (*paramIt).asUInt();
node_options.append_parameter_override("bridge." + bridge_key, uint16_t(bridge_port));
// cout << "bridge." << bridge_key << " = " << bridge_port << endl;
}
cout << "\t\tNode Name: " << node_name << endl;
// cout << item << endl;

std::shared_ptr<cloisim_ros::Base> node = nullptr;
rclcpp::NodeOptions node_options(default_node_options);

if (!node_type.compare("MICOM"))
{
if (isSingleMode)
{
node = std::make_shared<cloisim_ros::Micom>(node_options, node_name);
node_options.append_parameter_override("singlemode.robotname", item_name);
}
else

// store parameters
for (auto paramIt = item.begin(); paramIt != item.end(); ++paramIt)
{
node = std::make_shared<cloisim_ros::Micom>(node_options, node_name, item_name);
const auto bridge_key = paramIt.key().asString();
const auto bridge_port = (*paramIt).asUInt();
node_options.append_parameter_override("bridge." + bridge_key, uint16_t(bridge_port));
// cout << "bridge." << bridge_key << " = " << bridge_port << endl;
}
}
else if (!node_type.compare("LIDAR") || !node_type.compare("LASER"))
{
if (isSingleMode)
{

node = std::make_shared<cloisim_ros::Lidar>(node_options, node_name);
}
else
if (!node_type.compare("MICOM"))
{
node = std::make_shared<cloisim_ros::Lidar>(node_options, node_name, item_name);
if (isSingleMode)
{
node = std::make_shared<cloisim_ros::Micom>(node_options, node_name);
}
else
{
node = std::make_shared<cloisim_ros::Micom>(node_options, node_name, item_name);
}
}
}
else if (!node_type.compare("CAMERA"))
{
if (isSingleMode)
else if (!node_type.compare("LIDAR") || !node_type.compare("LASER"))
{
node = std::make_shared<cloisim_ros::Camera>(node_options, node_name);
if (isSingleMode)
{

node = std::make_shared<cloisim_ros::Lidar>(node_options, node_name);
}
else
{
node = std::make_shared<cloisim_ros::Lidar>(node_options, node_name, item_name);
}
}
else
else if (!node_type.compare("CAMERA"))
{
node = std::make_shared<cloisim_ros::Camera>(node_options, node_name, item_name);
if (isSingleMode)
{
node = std::make_shared<cloisim_ros::Camera>(node_options, node_name);
}
else
{
node = std::make_shared<cloisim_ros::Camera>(node_options, node_name, item_name);
}
}
}
else if (!node_type.compare("DEPTHCAMERA"))
{
if (isSingleMode)
else if (!node_type.compare("DEPTHCAMERA"))
{
node = std::make_shared<cloisim_ros::DepthCamera>(node_options, node_name);
if (isSingleMode)
{
node = std::make_shared<cloisim_ros::DepthCamera>(node_options, node_name);
}
else
{
node = std::make_shared<cloisim_ros::DepthCamera>(node_options, node_name, item_name);
}
}
else
else if (!node_type.compare("MULTICAMERA"))
{
node = std::make_shared<cloisim_ros::DepthCamera>(node_options, node_name, item_name);
if (isSingleMode)
{
node = std::make_shared<cloisim_ros::MultiCamera>(node_options, node_name);
}
else
{
node = std::make_shared<cloisim_ros::MultiCamera>(node_options, node_name, item_name);
}
}
}
else if (!node_type.compare("MULTICAMERA"))
{
if (isSingleMode)
else if (!node_type.compare("REALSENSE"))
{
node = std::make_shared<cloisim_ros::MultiCamera>(node_options, node_name);
if (isSingleMode)
{
node = std::make_shared<cloisim_ros::RealSense>(node_options, node_name);
}
else
{
node = std::make_shared<cloisim_ros::RealSense>(node_options, node_name, item_name);
}
}
else
else if (!node_type.compare("GPS"))
{
node = std::make_shared<cloisim_ros::MultiCamera>(node_options, node_name, item_name);
if (isSingleMode)
{
node = std::make_shared<cloisim_ros::Gps>(node_options, node_name);
}
else
{
node = std::make_shared<cloisim_ros::Gps>(node_options, node_name, item_name);
}
}
}
else if (!node_type.compare("REALSENSE"))
{
if (isSingleMode)
else if (!node_type.compare("ELEVATOR"))
{
node = std::make_shared<cloisim_ros::RealSense>(node_options, node_name);
node_options.append_parameter_override("model", item_name);
node = std::make_shared<cloisim_ros::ElevatorSystem>(node_options, node_name);
}
else
else if (!node_type.compare("WORLD"))
{
node = std::make_shared<cloisim_ros::RealSense>(node_options, node_name, item_name);
}
}
else if (!node_type.compare("GPS"))
{
if (isSingleMode)
{
node = std::make_shared<cloisim_ros::Gps>(node_options, node_name);
node_options.append_parameter_override("model", item_name);
node = std::make_shared<cloisim_ros::World>(node_options, node_name);
}
else
{
node = std::make_shared<cloisim_ros::Gps>(node_options, node_name, item_name);
cout << node_type << " is not supported." << endl;
continue;
}
}
else if (!node_type.compare("ELEVATOR"))
{
node_options.append_parameter_override("model", item_name);
node = std::make_shared<cloisim_ros::ElevatorSystem>(node_options, node_name);
}
else if (!node_type.compare("WORLD"))
{
node_options.append_parameter_override("model", item_name);
node = std::make_shared<cloisim_ros::World>(node_options, node_name);
}
else
{
cout << node_type << " is not supported." << endl;
continue;
}

node_list.push_back(node);
rclcpp_node_list.push_back(node);
}
}
}

for (auto it = node_list.begin(); it != node_list.end(); ++it)
for (auto it = rclcpp_node_list.begin(); it != rclcpp_node_list.end(); ++it)
{
executor.add_node(*it);
}
Expand Down
44 changes: 30 additions & 14 deletions cloisim_ros_realsense/src/realsense.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,16 +65,15 @@ void RealSense::Initialize()
}

image_transport::ImageTransport it(GetNode());

for (auto module : module_list_)
for (auto module_name : module_list_)
{
uint16_t portCamData, portCamInfo;
get_parameter_or("bridge." + module + "Data", portCamData, uint16_t(0));
get_parameter_or("bridge." + module + "Info", portCamInfo, uint16_t(0));
get_parameter_or("bridge." + module_name + "Data", portCamData, uint16_t(0));
get_parameter_or("bridge." + module_name + "Info", portCamInfo, uint16_t(0));

const auto topic_base_name_ = GetPartsName() + "/" + module;
const auto topic_base_name_ = GetPartsName() + "/" + module_name;

const auto hashKeySub = GetMainHashKey() + module;
const auto hashKeySub = GetMainHashKey() + module_name;

DBG_SIM_INFO("topic_name:%s, hash Key sub: %s", topic_base_name_.c_str(), hashKeySub.c_str());

Expand All @@ -83,11 +82,28 @@ void RealSense::Initialize()
dataPortMap_[bridgeIndex] = portCamData;
hashKeySubs_[bridgeIndex] = hashKeySub;

const auto topic_name = (module.find("depth") == string::npos)? "image_raw":"image_rect_raw";
const auto topic_name = (module_name.find("depth") == string::npos)? "image_raw":"image_rect_raw";
pubImages_[bridgeIndex] = it.advertiseCamera(topic_base_name_ + "/" + topic_name, 1);

// handling parameters for image_transport plugin
const auto format_value = get_parameter("format");
const auto jpeg_quality_value = get_parameter("jpeg_quality");
const auto png_level_value = get_parameter("png_level");

declare_parameters(module_name,
map<string, string>{
{"format", format_value.as_string()}});
declare_parameters(module_name,
map<string, int>{
{"jpeg_quality", jpeg_quality_value.as_int()},
{"png_level", png_level_value.as_int()}});

undeclare_parameter("format");
undeclare_parameter("jpeg_quality");
undeclare_parameter("png_level");

sensor_msgs::msg::Image msg_img;
msg_img.header.frame_id = module;
msg_img.header.frame_id = module_name;

msgImgs_[bridgeIndex] = msg_img;

Expand All @@ -99,13 +115,13 @@ void RealSense::Initialize()
if (pBridgeCamInfo != nullptr)
{
pBridgeCamInfo->Connect(zmq::Bridge::Mode::CLIENT, portCamInfo, hashKeySub + "Info");
const auto transform = GetObjectTransform(pBridgeCamInfo, module);
const auto child_frame_id = GetPartsName() + "_camera_" + module + "_frame";
const auto transform = GetObjectTransform(pBridgeCamInfo, module_name);
const auto child_frame_id = GetPartsName() + "_camera_" + module_name + "_frame";
SetupStaticTf2(transform, child_frame_id, header_frame_id);

cameraInfoManager_[bridgeIndex] = std::make_shared<camera_info_manager::CameraInfoManager>(GetNode().get());
const auto camSensorMsg = GetCameraSensorMessage(pBridgeCamInfo);
SetCameraInfoInManager(cameraInfoManager_[bridgeIndex], camSensorMsg, module);
SetCameraInfoInManager(cameraInfoManager_[bridgeIndex], camSensorMsg, module_name);
}

threads_.emplace_back(thread([=]() {
Expand Down Expand Up @@ -281,7 +297,7 @@ void RealSense::UpdateData(const uint bridge_index)
if (encoding_arg.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0)
{
// for only depth sensor(Z16)
for (uint i = 0; i < msg_img->data.size(); i += 2)
for (ulong i = 0; i < msg_img->data.size(); i += sizeof(short))
{
const auto depthDataInUInt16 = (uint16_t)tempImageData[i] << 8 | (uint16_t)tempImageData[i + 1];
const auto scaledDepthData = (double)depthDataInUInt16 / (double)UINT16_MAX;
Expand All @@ -295,7 +311,7 @@ void RealSense::UpdateData(const uint bridge_index)
else if (encoding_arg.compare(sensor_msgs::image_encodings::TYPE_16SC1) == 0)
{
// convert to little-endian
for (uint i = 0; i < msg_img->data.size(); i += 2)
for (ulong i = 0; i < msg_img->data.size(); i += sizeof(short))
{
const auto temp = tempImageData[i + 1];
tempImageData[i + 1] = tempImageData[i];
Expand All @@ -305,7 +321,7 @@ void RealSense::UpdateData(const uint bridge_index)
else if (encoding_arg.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
{
// convert to little-endian
for (uint i = 0; i < msg_img->data.size(); i += 4)
for (ulong i = 0; i < msg_img->data.size(); i += sizeof(float))
{
const auto temp3 = tempImageData[i + 3];
const auto temp2 = tempImageData[i + 2];
Expand Down

0 comments on commit 74a06d3

Please sign in to comment.