Pull request #9: ros2_driver update to release with Jacinto Robotics SDK

Merge in MITL/mmwave_ti_ros from ros2_update_robotics_sdk to master

* commit '1ffa862e94e72aad43a1701e7f0b7c30384d3237':
  [ros2_driver] Updated to get rid of error message at Ctrl+C termination
  [ros2_driver] Following updates made to ros2_driver/src/ti_mmwave_rospkg: 1. Updated CMakeLists.txt to install the "cfg" folder. 2. Path to .cfg file is now set using get_package_share_directory(). 3. Updated 6843*.py launch files to add a feature optionally disable rViz with an launch argument "rviz". To disable rViz, pass "rviz:=false" in the launch command.
This commit is contained in:
JuneChul Roh
2023-11-21 12:55:11 -06:00
committed by Pedrhom Nafisi
13 changed files with 326 additions and 244 deletions

View File

@@ -224,6 +224,10 @@ install(DIRECTORY
config
DESTINATION share/${PROJECT_NAME}
)
install(DIRECTORY
cfg
DESTINATION share/${PROJECT_NAME}
)
install(TARGETS
mmWaveCommSrv
DESTINATION lib/${PROJECT_NAME}

View File

@@ -8,14 +8,14 @@ from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Enter Path and Name Here
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/1443_Standard.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','1443_Standard.cfg')
device = "1443"
name = "/mmWaveCLI"
command_port = "/dev/ttyUSB0"
command_rate = "115200"
data_port = "/dev/ttyUSB1"
data_rate = "921600"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(
@@ -90,10 +90,9 @@ def generate_launch_description():
)
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
)
ld.add_action(global_param_node)

View File

@@ -8,14 +8,14 @@ from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Enter Path and Name Here
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/1642_Standard.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','1642_Standard.cfg')
name = "/mmWaveCLI"
device = "1642"
command_port = "/dev/ttyUSB0"
command_rate = "115200"
data_port = "/dev/ttyUSB1"
data_rate = "921600"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(
@@ -90,10 +90,9 @@ def generate_launch_description():
)
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
)
ld.add_action(global_param_node)

View File

@@ -8,14 +8,14 @@ from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Enter Path and Name Here
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/1843_Standard.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','1843_Standard.cfg')
device = "1843"
name = "/mmWaveCLI"
command_port = "/dev/ttyUSB0"
command_rate = "115200"
data_port = "/dev/ttyUSB1"
data_rate = "921600"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(
@@ -90,10 +90,9 @@ def generate_launch_description():
)
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
)
ld.add_action(global_param_node)

View File

@@ -11,14 +11,14 @@ from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Standard_Uncompressed.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','6432_Standard_Uncompressed.cfg')
device = "6432"
name = "/mmWaveCLI"
command_port = "/dev/ttyACM0"
command_rate = "115200"
data_port = "/dev/ttyACM0"
data_rate = "115200"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(

View File

@@ -1,23 +1,32 @@
import os
from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch import LaunchDescription, conditions
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Declare the launch argument
rviz_arg = DeclareLaunchArgument(
'rviz',
default_value='true',
description='Enable RViz'
)
# Use the launch argument in the condition for adding the RViz node
rviz_enabled = LaunchConfiguration('rviz')
# Enter Path and Name Here
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Standard.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','6843AOP_Standard.cfg')
device = "6843"
name = "/mmWaveCLI"
command_port = "/dev/ttyUSB0"
command_rate = "115200"
data_port = "/dev/ttyUSB1"
data_rate = "921600"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(
my_package_dir,
'config',
@@ -32,21 +41,21 @@ def generate_launch_description():
)
mmWaveCommSrv = Node(
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
]
)
@@ -89,13 +98,16 @@ def generate_launch_description():
]
)
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')],
condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true
)
ld = LaunchDescription()
ld.add_action(rviz_arg)
ld.add_action(global_param_node)
ld.add_action(mmWaveCommSrv)
ld.add_action(mmWaveQuickConfig)

View File

@@ -1,23 +1,32 @@
import os
from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch import LaunchDescription, conditions
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Declare the launch argument
rviz_arg = DeclareLaunchArgument(
'rviz',
default_value='true',
description='Enable RViz'
)
# Use the launch argument in the condition for adding the RViz node
rviz_enabled = LaunchConfiguration('rviz')
# Enter Path and Name Here
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Tracking.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','6843AOP_Tracking.cfg')
device = "6843"
name = "/mmWaveCLI"
command_port = "/dev/ttyUSB0"
command_rate = "115200"
data_port = "/dev/ttyUSB1"
data_rate = "921600"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(
my_package_dir,
'config',
@@ -32,21 +41,21 @@ def generate_launch_description():
)
mmWaveCommSrv = Node(
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
]
)
@@ -89,13 +98,16 @@ def generate_launch_description():
]
)
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')],
condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true
)
ld = LaunchDescription()
ld.add_action(rviz_arg)
ld.add_action(global_param_node)
ld.add_action(mmWaveCommSrv)
ld.add_action(mmWaveQuickConfig)

View File

@@ -1,23 +1,32 @@
import os
from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch import LaunchDescription, conditions
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Declare the launch argument
rviz_arg = DeclareLaunchArgument(
'rviz',
default_value='true',
description='Enable RViz'
)
# Use the launch argument in the condition for adding the RViz node
rviz_enabled = LaunchConfiguration('rviz')
# Enter Path and Name Here
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843ISK_Occupancy.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','6843ISK_Occupancy.cfg')
device = "6843"
name = "/mmWaveCLI"
command_port = "/dev/ttyUSB0"
command_rate = "115200"
data_port = "/dev/ttyUSB1"
data_rate = "921600"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(
my_package_dir,
'config',
@@ -32,21 +41,21 @@ def generate_launch_description():
)
mmWaveCommSrv = Node(
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
]
)
@@ -89,13 +98,16 @@ def generate_launch_description():
]
)
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')],
condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true
)
ld = LaunchDescription()
ld.add_action(rviz_arg)
ld.add_action(global_param_node)
ld.add_action(mmWaveCommSrv)
ld.add_action(mmWaveQuickConfig)

View File

@@ -1,23 +1,32 @@
import os
from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch import LaunchDescription, conditions
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Declare the launch argument
rviz_arg = DeclareLaunchArgument(
'rviz',
default_value='true',
description='Enable RViz'
)
# Use the launch argument in the condition for adding the RViz node
rviz_enabled = LaunchConfiguration('rviz')
# Enter Path and Name Here
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843ISK_Standard.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','6843ISK_Standard.cfg')
device = "6843"
name = "/mmWaveCLI"
command_port = "/dev/ttyUSB0"
command_rate = "115200"
data_port = "/dev/ttyUSB1"
data_rate = "921600"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(
my_package_dir,
'config',
@@ -32,21 +41,21 @@ def generate_launch_description():
)
mmWaveCommSrv = Node(
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
]
)
@@ -89,13 +98,16 @@ def generate_launch_description():
]
)
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')],
condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true
)
ld = LaunchDescription()
ld.add_action(rviz_arg)
ld.add_action(global_param_node)
ld.add_action(mmWaveCommSrv)
ld.add_action(mmWaveQuickConfig)

View File

@@ -1,23 +1,32 @@
import os
from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch import LaunchDescription, conditions
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Declare the launch argument
rviz_arg = DeclareLaunchArgument(
'rviz',
default_value='true',
description='Enable RViz'
)
# Use the launch argument in the condition for adding the RViz node
rviz_enabled = LaunchConfiguration('rviz')
# Enter Path and Name Here
path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Tracking.cfg";
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
path = os.path.join(my_package_dir,'cfg','6843AOP_Tracking.cfg')
device = "6843"
name = "/mmWaveCLI"
command_port = "/dev/ttyUSB0"
command_rate = "115200"
data_port = "/dev/ttyUSB1"
data_rate = "921600"
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
ld = LaunchDescription()
ConfigParameters = os.path.join(
my_package_dir,
'config',
@@ -32,21 +41,21 @@ def generate_launch_description():
)
mmWaveCommSrv = Node(
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
package="ti_mmwave_rospkg",
executable="mmWaveCommSrv",
name="mmWaveCommSrv",
output="screen",
emulate_tty=True,
parameters=[
{"command_port": command_port},
{"command_rate": command_rate},
{"data_port": data_port},
{"data_rate": data_rate},
{"max_allowed_elevation_angle_deg": "90"},
{"max_allowed_azimuth_angle_deg": "90"},
{"frame_id": "/ti_mmwave_0"},
{"mmwavecli_name": name},
{"mmwavecli_cfg": path}
]
)
@@ -89,13 +98,16 @@ def generate_launch_description():
]
)
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')],
condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true
)
ld = LaunchDescription()
ld.add_action(rviz_arg)
ld.add_action(global_param_node)
ld.add_action(mmWaveCommSrv)
ld.add_action(mmWaveQuickConfig)

View File

@@ -0,0 +1,21 @@
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Enter Path and Name Here
my_package_dir = get_package_share_directory('ti_mmwave_rospkg')
Rviz2 = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')]
)
ld = LaunchDescription([
Rviz2
])
return ld

View File

@@ -10,7 +10,6 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
using namespace std::chrono_literals;
std::shared_ptr<rclcpp::Node> node = nullptr;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr DataUARTHandler_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarScan>::SharedPtr radar_scan_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarOccupancy>::SharedPtr radar_occupancy_pub;
@@ -39,6 +38,8 @@ void DataUARTHandler::sigHandler(int32_t sig)
}
rclcpp::shutdown();
exit(0);
}
struct mmWaveCloudType
@@ -1256,7 +1257,7 @@ void DataUARTHandler::stop()
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<DataUARTHandlerNode>();
auto node = std::make_shared<DataUARTHandlerNode>();
std::string mySerialPort = node->get_parameter("data_port").as_string();
std::string myBaudRate = node->get_parameter("data_rate").as_string();
std::string myFrameID = "/ti_mmwave_0";

View File

@@ -17,7 +17,6 @@
#include <vector>
using namespace std::chrono_literals;
std::shared_ptr<rclcpp::Node> nodeptr = nullptr;
class mmWaveQuickConfig : public rclcpp::Node
{
@@ -35,7 +34,7 @@ private:
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
nodeptr = std::make_shared<mmWaveQuickConfig>();
auto nodeptr = std::make_shared<mmWaveQuickConfig>();
rclcpp::Client<ti_mmwave_rospkg_msgs::srv::MmwaveCli>::SharedPtr client = nodeptr->create_client<ti_mmwave_rospkg_msgs::srv::MmwaveCli>("/ti_mmwave_rospkg_msgs/mmwave_cli");
auto request = std::make_shared<ti_mmwave_rospkg_msgs::srv::MmwaveCli::Request>();
auto response = std::make_shared<ti_mmwave_rospkg_msgs::srv::MmwaveCli::Response>();