diff --git a/README.md b/README.md index fc5650a1ccfcacd6a25fba90359d807c794f03ca..f284531be5b2471bc7eddddcffb7dcfb3f0d7a2f 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal). ## Installation -#### External dependencies +#### IRI dependencies * **bno055_imu_driver**: This is the [bno055 imu driver](https://gitlab.iri.upc.edu/labrobotica/drivers/bno055_imu_driver). To install it run the following commands: diff --git a/launch/collsions.launch b/launch/collsions.launch new file mode 100644 index 0000000000000000000000000000000000000000..c4c2f6e778e3e7fc186c6b5f4c5433d37b7093c4 --- /dev/null +++ b/launch/collsions.launch @@ -0,0 +1,78 @@ +<?xml version="1.0" encoding="UTF-8"?> +<launch> + + <arg name="output" default="screen"/> + <arg name="launch_prefix" default=""/> + <arg name="dr" default="false"/> + <arg name="sim_time" default="false"/> + <param name="/use_sim_time" value="$(arg sim_time)" /> + <arg name="imu_driver" default="true"/> + + <!-- IMU driver parameters --> + <arg name="imu_config_file" default="$(find iri_collision_manager)/config/bno055.yaml" /> + <arg name="imu_calibration_file" default="$(find iri_collision_manager)/calibration/bno055.cal" /> + + <!-- IMU bias filter parameters --> + <arg name="bias_filter_config_file" default="$(find iri_imu_bias_filter)/config/params.yaml"/> + <arg name="imu_raw_topic" default="/bno055_imu/imu"/> + <arg name="imu_filtered_topic" default="/bno055_imu/imu_bias_filtered"/> + <arg name="cmd_vel_topic" default="/cmd_vel"/> + <arg name="update_bias_ns" default="~/update_bias"/> + + <!-- Laser Scan ICP parameters --> + <arg name="icp_config_file" default="$(find iri_laser_scan_icp)/config/params.yaml"/> + <arg name="icp_service_name" default="/iri_collision_manager/icp"/> + + <!-- Collision manager parameters --> + <arg name="collision_config_file" default="$(find iri_collision_manager)/config/params.yaml"/> + <arg name="front_laser_topic" default="/front_laser_scan"/> + <arg name="rear_laser_topic" default="/rear_laser_scan"/> + + <include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch" + if="$(arg imu_driver)"> + <arg name="config_file" value="$(arg imu_config_file)"/> + <arg name="calibration_file" value="$(arg imu_calibration_file)"/> + <arg name="node_name" value="bno055_imu"/> + <arg name="output" value="$(arg output)"/> + <arg name="launch_prefix" value="$(arg launch_prefix)"/> + </include> + + <include file="$(find iri_imu_bias_filter)/launch/node.launch"> + <arg name="node_name" value="iri_imu_bias_filter"/> + <arg name="output" value="$(arg output)"/> + <arg name="launch_prefix" value="$(arg launch_prefix)"/> + <arg name="config_file" value="$(arg bias_filter_config_file)"/> + <arg name="imu_topic_in" value="$(arg imu_raw_topic)"/> + <arg name="imu_topic_out" value="$(arg imu_filtered_topic)"/> + <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/> + <arg name="update_bias_ns" value="$(arg update_bias_ns)"/> + </include> + + <include file="$(find iri_laser_scan_icp)/launch/node.launch"> + <arg name="node_name" value="iri_laser_scan_icp"/> + <arg name="output" value="$(arg output)"/> + <arg name="launch_prefix" value="$(arg launch_prefix)"/> + <arg name="config_file" value="$(arg icp_config_file)"/> + <arg name="icp_service_name" value="$(arg icp_service_name)"/> + </include> + + <include file="$(find iri_collision_manager)/launch/node.launch"> + <arg name="node_name" value="iri_collision_manager"/> + <arg name="output" value="$(arg output)"/> + <arg name="launch_prefix" value="$(arg launch_prefix)"/> + <arg name="config_file" value="$(arg collision_config_file)"/> + <arg name="imu_topic" value="$(arg imu_filtered_topic)"/> + <arg name="front_laser_topic" value="$(arg front_laser_topic)"/> + <arg name="rear_laser_topic" value="$(arg rear_laser_topic)"/> + <arg name="icp_service_name" value="$(arg icp_service_name)"/> + </include> + + + <node name="rqt_reconfigure_iri_collision_manager" + pkg ="rqt_reconfigure" + type="rqt_reconfigure" + if ="$(arg dr)" + args="iri_collision_manager"> + </node> + +</launch> diff --git a/launch/imu_and_collisions.launch b/launch/imu_and_collisions.launch deleted file mode 100644 index 033a47cf8eeda84d66979bb305d11b3c765c1e01..0000000000000000000000000000000000000000 --- a/launch/imu_and_collisions.launch +++ /dev/null @@ -1,40 +0,0 @@ -<?xml version="1.0" encoding="UTF-8"?> -<launch> - - <arg name="output" default="screen"/> - <arg name="launch_prefix" default=""/> - <arg name="dr" default="false"/> - <arg name="sim_time" default="false"/> - <param name="/use_sim_time" value="$(arg sim_time)" /> - - - <arg name="collision_config_file" default="$(find iri_collision_manager)/config/params.yaml"/> - <arg name="imu_topic" default="/bno055_imu/imu"/> - - <arg name="imu_config_file" default="$(find iri_collision_manager)/config/bno055.yaml" /> - <arg name="imu_calibration_file" default="$(find iri_collision_manager)/calibration/bno055.cal" /> - - <include file="$(find iri_collision_manager)/launch/node.launch"> - <arg name="node_name" value="iri_collision_manager"/> - <arg name="output" value="$(arg output)"/> - <arg name="launch_prefix" value="$(arg launch_prefix)"/> - <arg name="config_file" value="$(arg collision_config_file)"/> - <arg name="imu_topic" value="$(arg imu_topic)"/> - </include> - - <include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch"> - <arg name="config_file" value="$(arg imu_config_file)"/> - <arg name="calibration_file" value="$(arg imu_calibration_file)"/> - <arg name="node_name" value="bno055_imu"/> - <arg name="output" value="$(arg output)"/> - <arg name="launch_prefix" value="$(arg launch_prefix)"/> - </include> - - <node name="rqt_reconfigure_iri_collision_manager" - pkg ="rqt_reconfigure" - type="rqt_reconfigure" - if ="$(arg dr)" - args="iri_collision_manager"> - </node> - -</launch> diff --git a/launch/node.launch b/launch/node.launch index f187b04dac8a8986547f96ada971edf386a91d8f..7e9455883fe50399ee9e1a0cb10fe73db9a0465c 100644 --- a/launch/node.launch +++ b/launch/node.launch @@ -7,6 +7,9 @@ <arg name="config_file" default="$(find iri_collision_manager)/config/params.yaml"/> <!-- <arg name="topic_name" default="new_topic_name"/> --> <arg name="imu_topic" default="/bno055_imu/imu"/> + <arg name="front_laser_topic" default="~/front_laser_scan"/> + <arg name="rear_laser_topic" default="~/rear_laser_scan"/> + <arg name="icp_service_name" default="~/icp"/> <node name="$(arg node_name)" pkg ="iri_collision_manager" @@ -16,6 +19,9 @@ <rosparam file="$(arg config_file)" command="load"/> <!--<remap from="~/topic" to="$(arg topic_name)"/>--> <remap from="~/imu" to="$(arg imu_topic)"/> + <remap from="~/front_laser_scan" to="$(arg front_laser_topic)"/> + <remap from="~/rear_laser_scan" to="$(arg rear_laser_topic)"/> + <remap from="~/icp" to="$(arg icp_service_name)"/> </node> </launch> diff --git a/launch/test.launch b/launch/test.launch index cdc21d29f953ef144ea54f6411df0916443db1c7..ffe8b8ff8856de5baa194b57c876db5e1bc5b5d4 100644 --- a/launch/test.launch +++ b/launch/test.launch @@ -16,6 +16,9 @@ <arg name="launch_prefix" value="$(arg launch_prefix)"/> <arg name="config_file" value="$(arg collision_config_file)"/> <arg name="imu_topic" value="$(arg imu_topic)"/> + <arg name="front_laser_topic" value="/front_laser_scan"/> + <arg name="rear_laser_topic" value="/rear_laser_scan"/> + <arg name="icp_service_name" value="~/icp"/> </include> <node name="rqt_reconfigure_iri_collision_manager"