Made parameters on parameter server private. Small workaround for DCM stiffness not turning on.
This commit is contained in:
Konstantinos Chatzilygeroudis
parent
07d263ff23
commit
ba0588aca2
@ -122,13 +122,14 @@ void NaoCamera::disconnect()
|
||||
|
||||
void NaoCamera::loadParams()
|
||||
{
|
||||
node_handle_.param("Nao_UseCamera", camera_enabled_, true);
|
||||
node_handle_.param("Nao_TopCameraEnabled", camera_top_enabled_, camera_enabled_);
|
||||
node_handle_.param("Nao_BottomCameraEnabled", camera_bottom_enabled_, camera_enabled_);
|
||||
ros::NodeHandle n_p("~");
|
||||
n_p.param("UseCamera", camera_enabled_, true);
|
||||
n_p.param("TopCameraEnabled", camera_top_enabled_, camera_enabled_);
|
||||
n_p.param("BottomCameraEnabled", camera_bottom_enabled_, camera_enabled_);
|
||||
|
||||
node_handle_.param("Nao_TopicQueue", topic_queue_, 50);
|
||||
n_p.param("TopicQueue", topic_queue_, 50);
|
||||
|
||||
node_handle_.param("Nao_Prefix", prefix_, string("nao_dcm"));
|
||||
n_p.param("Prefix", prefix_, string("nao_dcm"));
|
||||
prefix_ = prefix_+"/";
|
||||
}
|
||||
|
||||
@ -379,6 +380,7 @@ int main( int argc, char** argv )
|
||||
string pip = "127.0.0.1";
|
||||
ros::init(argc, argv, "nao_dcm_camera");
|
||||
ros::NodeHandle n;
|
||||
ros::NodeHandle n_p("~");
|
||||
ros::NodeHandle top("CameraTopNode"), bottom("CameraBottomNode");
|
||||
if(!ros::master::check())
|
||||
{
|
||||
@ -394,11 +396,11 @@ int main( int argc, char** argv )
|
||||
double communication_rate;
|
||||
|
||||
// Load Params from Parameter Server
|
||||
n.param("Nao_RobotIP", pip, string("127.0.0.1"));
|
||||
n.param("Nao_RobotPort", pport,9559);
|
||||
n.param("Nao_CameraBrokerPort", broker_port, 54001);
|
||||
n.param("Nao_CameraBrokerIP", broker_ip, string("0.0.0.0"));
|
||||
n.param("Nao_CameraFrequency", communication_rate, 30.0);
|
||||
n_p.param("RobotIP", pip, string("127.0.0.1"));
|
||||
n_p.param("RobotPort", pport,9559);
|
||||
n_p.param("CameraBrokerPort", broker_port, 54001);
|
||||
n_p.param("CameraBrokerIP", broker_ip, string("0.0.0.0"));
|
||||
n_p.param("CameraFrequency", communication_rate, 30.0);
|
||||
|
||||
// Create your own broker
|
||||
boost::shared_ptr<AL::ALBroker> broker;
|
||||
|
||||
@ -1,36 +1,36 @@
|
||||
# DO NOT CHANGE IT
|
||||
Nao_Version: V4
|
||||
Version: V4
|
||||
|
||||
# Choose what components are enabled on startup
|
||||
Nao_SonarEnabled: true
|
||||
Nao_TactilesEnabled: true
|
||||
Nao_BumpersEnabled: true
|
||||
Nao_FootContactsEnabled: true
|
||||
SonarEnabled: true
|
||||
TactilesEnabled: true
|
||||
BumpersEnabled: true
|
||||
FootContactsEnabled: true
|
||||
|
||||
# Camera related parameters
|
||||
Nao_UseCamera: true
|
||||
Nao_TopCameraEnabled: true
|
||||
Nao_BottomCameraEnabled: true
|
||||
UseCamera: true
|
||||
TopCameraEnabled: true
|
||||
BottomCameraEnabled: true
|
||||
|
||||
# Application related parameters
|
||||
Nao_PublishIMU: true
|
||||
PublishIMU: true
|
||||
|
||||
# ROS related paramaters
|
||||
Nao_TopicQueue: 10
|
||||
TopicQueue: 10
|
||||
|
||||
# Robot-Connection related parameters
|
||||
Nao_RobotPort: 9559
|
||||
Nao_RobotIP: nao.local
|
||||
Nao_DriverBrokerPort: 54000
|
||||
Nao_DriverBrokerIP: 0.0.0.0
|
||||
Nao_CameraBrokerPort: 54001
|
||||
Nao_CameraBrokerIP: 0.0.0.0
|
||||
RobotPort: 9559
|
||||
RobotIP: nao.local
|
||||
DriverBrokerPort: 54000
|
||||
DriverBrokerIP: 0.0.0.0
|
||||
CameraBrokerPort: 54001
|
||||
CameraBrokerIP: 0.0.0.0
|
||||
|
||||
# Communication related parameters
|
||||
Nao_HighCommunicationFrequency: 50
|
||||
Nao_LowCommunicationFrequency: 10
|
||||
Nao_ControllerFrequency: 10
|
||||
Nao_CameraFrequency: 15
|
||||
HighCommunicationFrequency: 25
|
||||
LowCommunicationFrequency: 10
|
||||
ControllerFrequency: 10
|
||||
CameraFrequency: 15
|
||||
|
||||
# DO NOT CHANGE IT
|
||||
Nao_Prefix: nao_dcm
|
||||
Prefix: nao_dcm
|
||||
@ -3,56 +3,54 @@
|
||||
<!-- Call Nao Robot publisher -->
|
||||
<include file="$(find nao_dcm_description)/launch/nao_dcm_H21_urdf.launch" />
|
||||
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<!-- Nao Diagnostics -->
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
|
||||
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
|
||||
</node>
|
||||
|
||||
<!-- Call Nao Robot Trajectory Controller -->
|
||||
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H21.launch"/>
|
||||
|
||||
<!-- Do Not Run Dashboard -->
|
||||
<!-- <node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/> -->
|
||||
|
||||
<!-- Call Nao Robot Driver -->
|
||||
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
|
||||
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
|
||||
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
|
||||
|
||||
<!-- Nao Diagnostics -->
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
|
||||
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
|
||||
</node>
|
||||
|
||||
<!-- Specify Robot Body Type on Parameter Server -->
|
||||
<param name="Nao_BodyType" value="H21"/>
|
||||
<param name="BodyType" value="H21"/>
|
||||
</node>
|
||||
|
||||
<!-- Call Nao Robot Trajectory Controller -->
|
||||
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H21.launch"/>
|
||||
<!-- Call Nao Camera -->
|
||||
<!-- <node pkg="nao_dcm_driver" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
|
||||
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
|
||||
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
|
||||
</node> -->
|
||||
|
||||
<!-- Run Dashboard -->
|
||||
<!-- <node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/> -->
|
||||
<!-- Call helper nodes to get visual odometry and combine it with imu data -->
|
||||
<!-- <node name="viso" pkg="viso2_ros" type="mono_odometer" respawn="false" output="screen">
|
||||
<param name="sensor_frame_id" value="/CameraTop"/>
|
||||
<param name="publish_tf" value="false"/>
|
||||
<remap from="image" to="/nao_dcm/CameraTop/image_raw"/>
|
||||
<remap from="/viso/odometry" to="/nao_dcm/odom"/>
|
||||
</node> -->
|
||||
|
||||
<!-- Call Nao Robot Driver -->
|
||||
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
|
||||
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
|
||||
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
|
||||
</node>
|
||||
|
||||
<!-- Call Nao Camera -->
|
||||
<!-- <node pkg="nao_dcm_driver" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
|
||||
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
|
||||
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
|
||||
</node> -->
|
||||
|
||||
<!-- Call helper nodes to get visual odometry and combine it with imu data -->
|
||||
<!-- <node name="viso" pkg="viso2_ros" type="mono_odometer" respawn="false" output="screen">
|
||||
<param name="sensor_frame_id" value="/CameraTop"/>
|
||||
<param name="publish_tf" value="false"/>
|
||||
<remap from="image" to="/nao_dcm/CameraTop/image_raw"/>
|
||||
<remap from="/viso/odometry" to="/nao_dcm/odom"/>
|
||||
</node> -->
|
||||
|
||||
<!-- <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
|
||||
<param name="output_frame" value="odom_combined"/>
|
||||
<param name="base_footprint_frame" value="base_link"/>
|
||||
<param name="freq" value="100.0"/>
|
||||
<param name="sensor_timeout" value="1.0"/>
|
||||
<param name="odom_used" value="true"/>
|
||||
<param name="imu_used" value="true"/>
|
||||
<param name="vo_used" value="false"/>
|
||||
<param name="debug" value="false"/>
|
||||
<param name="self_diagnose" value="false"/>
|
||||
<remap from="imu_data" to="nao_dcm/imu_data"/>
|
||||
<remap from="odom" to="nao_dcm/odom"/>
|
||||
<remap from="robot_pose_ekf/odom_combined" to="nao_dcm/odom_combined"/>
|
||||
</node> -->
|
||||
<!-- <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
|
||||
<param name="output_frame" value="odom_combined"/>
|
||||
<param name="base_footprint_frame" value="base_link"/>
|
||||
<param name="freq" value="100.0"/>
|
||||
<param name="sensor_timeout" value="1.0"/>
|
||||
<param name="odom_used" value="true"/>
|
||||
<param name="imu_used" value="true"/>
|
||||
<param name="vo_used" value="false"/>
|
||||
<param name="debug" value="false"/>
|
||||
<param name="self_diagnose" value="false"/>
|
||||
<remap from="imu_data" to="nao_dcm/imu_data"/>
|
||||
<remap from="odom" to="nao_dcm/odom"/>
|
||||
<remap from="robot_pose_ekf/odom_combined" to="nao_dcm/odom_combined"/>
|
||||
</node> -->
|
||||
|
||||
</launch>
|
||||
@ -3,33 +3,35 @@
|
||||
<!-- Call Nao Robot publisher -->
|
||||
<include file="$(find nao_dcm_description)/launch/nao_dcm_H21_urdf.launch" />
|
||||
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<!-- Nao Diagnostics -->
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
|
||||
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
|
||||
</node>
|
||||
|
||||
<!-- Call Nao Robot Trajectory Controller -->
|
||||
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H21.launch"/>
|
||||
|
||||
<!-- Run Dashboard -->
|
||||
<node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/>
|
||||
|
||||
<!-- Call Nao Robot Driver -->
|
||||
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
|
||||
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
|
||||
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
|
||||
|
||||
<!-- Nao Diagnostics -->
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
|
||||
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
|
||||
</node>
|
||||
|
||||
<!-- Specify Robot Body Type on Parameter Server -->
|
||||
<param name="Nao_BodyType" value="H21"/>
|
||||
<param name="BodyType" value="H21"/>
|
||||
</node>
|
||||
|
||||
<!-- Call Nao Robot Trajectory Controller -->
|
||||
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H21.launch"/>
|
||||
|
||||
<!-- Run Dashboard -->
|
||||
<node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/>
|
||||
|
||||
<!-- Call Nao Robot Driver -->
|
||||
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
|
||||
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
|
||||
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
|
||||
</node>
|
||||
|
||||
<!-- Call Nao Camera -->
|
||||
<node pkg="nao_dcm_camera" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
|
||||
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
|
||||
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
|
||||
</node>
|
||||
<!-- Call Nao Camera -->
|
||||
<node pkg="nao_dcm_camera" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
|
||||
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
|
||||
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
|
||||
<!-- Specify Robot Body Type on Parameter Server -->
|
||||
<param name="BodyType" value="H21"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,58 +1,56 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Call Nao Robot publisher -->
|
||||
<include file="$(find nao_dcm_description)/launch/nao_dcm_H25_urdf.launch" />
|
||||
<include file="$(find nao_dcm_description)/launch/nao_dcm_H25_urdf.launch" />
|
||||
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
|
||||
<!-- Nao Diagnostics -->
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
|
||||
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
|
||||
</node>
|
||||
|
||||
<!-- Nao Diagnostics -->
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
|
||||
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
|
||||
</node>
|
||||
<!-- Call Nao Robot Trajectory Controller -->
|
||||
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H25.launch"/>
|
||||
|
||||
<!-- Specify Robot Body Type on Parameter Server -->
|
||||
<param name="Nao_BodyType" value="H25"/>
|
||||
<!-- Do Not Run Dashboard -->
|
||||
<!-- <node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/> -->
|
||||
|
||||
<!-- Call Nao Robot Trajectory Controller -->
|
||||
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H25.launch"/>
|
||||
<!-- Call Nao Robot Driver -->
|
||||
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
|
||||
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
|
||||
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
|
||||
<!-- Specify Robot Body Type on Parameter Server -->
|
||||
<param name="BodyType" value="H25"/>
|
||||
</node>
|
||||
|
||||
<!-- Run Dashboard -->
|
||||
<!-- <node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/> -->
|
||||
<!-- Call Nao Camera -->
|
||||
<!-- <node pkg="nao_dcm_driver" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
|
||||
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
|
||||
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
|
||||
</node> -->
|
||||
|
||||
<!-- Call Nao Robot Driver -->
|
||||
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
|
||||
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
|
||||
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
|
||||
</node>
|
||||
<!-- Call helper nodes to get visual odometry and combine it with imu data -->
|
||||
<!-- <node name="viso" pkg="viso2_ros" type="mono_odometer" respawn="false" output="screen">
|
||||
<param name="sensor_frame_id" value="/CameraTop"/>
|
||||
<param name="publish_tf" value="false"/>
|
||||
<remap from="image" to="/nao_dcm/CameraTop/image_raw"/>
|
||||
<remap from="/viso/odometry" to="/nao_dcm/odom"/>
|
||||
</node>
|
||||
|
||||
<!-- Call Nao Camera -->
|
||||
<!-- <node pkg="nao_dcm_driver" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
|
||||
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
|
||||
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
|
||||
</node> -->
|
||||
|
||||
<!-- Call helper nodes to get visual odometry and combine it with imu data -->
|
||||
<!-- <node name="viso" pkg="viso2_ros" type="mono_odometer" respawn="false" output="screen">
|
||||
<param name="sensor_frame_id" value="/CameraTop"/>
|
||||
<param name="publish_tf" value="false"/>
|
||||
<remap from="image" to="/nao_dcm/CameraTop/image_raw"/>
|
||||
<remap from="/viso/odometry" to="/nao_dcm/odom"/>
|
||||
</node>
|
||||
|
||||
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
|
||||
<param name="output_frame" value="odom_combined"/>
|
||||
<param name="base_footprint_frame" value="base_link"/>
|
||||
<param name="freq" value="100.0"/>
|
||||
<param name="sensor_timeout" value="1.0"/>
|
||||
<param name="odom_used" value="true"/>
|
||||
<param name="imu_used" value="true"/>
|
||||
<param name="vo_used" value="false"/>
|
||||
<param name="debug" value="false"/>
|
||||
<param name="self_diagnose" value="false"/>
|
||||
<remap from="imu_data" to="nao_dcm/imu_data"/>
|
||||
<remap from="odom" to="nao_dcm/odom"/>
|
||||
<remap from="robot_pose_ekf/odom_combined" to="nao_dcm/odom_combined"/>
|
||||
</node> -->
|
||||
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
|
||||
<param name="output_frame" value="odom_combined"/>
|
||||
<param name="base_footprint_frame" value="base_link"/>
|
||||
<param name="freq" value="100.0"/>
|
||||
<param name="sensor_timeout" value="1.0"/>
|
||||
<param name="odom_used" value="true"/>
|
||||
<param name="imu_used" value="true"/>
|
||||
<param name="vo_used" value="false"/>
|
||||
<param name="debug" value="false"/>
|
||||
<param name="self_diagnose" value="false"/>
|
||||
<remap from="imu_data" to="nao_dcm/imu_data"/>
|
||||
<remap from="odom" to="nao_dcm/odom"/>
|
||||
<remap from="robot_pose_ekf/odom_combined" to="nao_dcm/odom_combined"/>
|
||||
</node> -->
|
||||
|
||||
</launch>
|
||||
@ -3,33 +3,35 @@
|
||||
<!-- Call Nao Robot publisher -->
|
||||
<include file="$(find nao_dcm_description)/launch/nao_dcm_H25_urdf.launch" />
|
||||
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
|
||||
<!-- Nao Diagnostics -->
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
|
||||
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
|
||||
</node>
|
||||
|
||||
<!-- Nao Diagnostics -->
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
|
||||
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
|
||||
</node>
|
||||
<!-- Call Nao Robot Trajectory Controller -->
|
||||
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H25.launch"/>
|
||||
|
||||
<!-- Specify Robot Body Type on Parameter Server -->
|
||||
<param name="Nao_BodyType" value="H25"/>
|
||||
<!-- Run Dashboard -->
|
||||
<node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/>
|
||||
|
||||
<!-- Call Nao Robot Trajectory Controller -->
|
||||
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H25.launch"/>
|
||||
<!-- Call Nao Robot Driver -->
|
||||
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
|
||||
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
|
||||
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
|
||||
<!-- Specify Robot Body Type on Parameter Server -->
|
||||
<param name="BodyType" value="H25"/>
|
||||
</node>
|
||||
|
||||
<!-- Run Dashboard -->
|
||||
<node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/>
|
||||
|
||||
<!-- Call Nao Robot Driver -->
|
||||
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
|
||||
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
|
||||
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
|
||||
</node>
|
||||
|
||||
<!-- Call Nao Camera -->
|
||||
<node pkg="nao_dcm_camera" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
|
||||
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
|
||||
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
|
||||
</node>
|
||||
<!-- Call Nao Camera -->
|
||||
<node pkg="nao_dcm_camera" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
|
||||
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
|
||||
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
|
||||
<!-- Load configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
|
||||
<!-- Specify Robot Body Type on Parameter Server -->
|
||||
<param name="BodyType" value="H25"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -265,9 +265,16 @@ bool Nao::initialize()
|
||||
|
||||
// Create Joints Hardness Alias
|
||||
commandAlias[0] = string("JointsHardness");
|
||||
commandAlias[1].arraySetSize(number_of_joints_-1);
|
||||
int k = 0;
|
||||
for(int i=0;i<number_of_joints_;i++)
|
||||
{
|
||||
commandAlias[1][i] = string("Device/SubDeviceList/"+joint_names_[i]+"/Hardness/Actuator/Value");
|
||||
if(joint_names_[i] == "RHipYawPitch")
|
||||
{
|
||||
k = -1;
|
||||
i++;
|
||||
}
|
||||
commandAlias[1][i+k] = string("Device/SubDeviceList/"+joint_names_[i]+"/Hardness/Actuator/Value");
|
||||
}
|
||||
dcm_proxy_.createAlias(commandAlias);
|
||||
|
||||
@ -287,8 +294,8 @@ bool Nao::initialize()
|
||||
}
|
||||
|
||||
// Turn Stiffness On
|
||||
vector<float> stiff = vector<float>(number_of_joints_,1.0f);
|
||||
vector<int> times = vector<int>(number_of_joints_,0);
|
||||
vector<float> stiff = vector<float>(number_of_joints_-1,1.0f);
|
||||
vector<int> times = vector<int>(number_of_joints_-1,0);
|
||||
DCMAliasTimedCommand("JointsHardness",stiff, times);
|
||||
stiffnesses_enabled_ = true;
|
||||
|
||||
@ -460,25 +467,26 @@ void Nao::subscribe()
|
||||
|
||||
void Nao::loadParams()
|
||||
{
|
||||
ros::NodeHandle n_p("~");
|
||||
// Load Server Parameters
|
||||
node_handle_.param("Nao_Version", version_, string("V4"));
|
||||
node_handle_.param("Nao_BodyType", body_type_, string("H21"));
|
||||
n_p.param("Version", version_, string("V4"));
|
||||
n_p.param("BodyType", body_type_, string("H21"));
|
||||
|
||||
node_handle_.param("Nao_TactilesEnabled", tactiles_enabled_, true);
|
||||
node_handle_.param("Nao_BumpersEnabled", bumpers_enabled_, true);
|
||||
node_handle_.param("Nao_SonarEnabled", sonar_enabled_, true);
|
||||
node_handle_.param("Nao_FootContactsEnabled", foot_contacts_enabled_, true);
|
||||
n_p.param("TactilesEnabled", tactiles_enabled_, true);
|
||||
n_p.param("BumpersEnabled", bumpers_enabled_, true);
|
||||
n_p.param("SonarEnabled", sonar_enabled_, true);
|
||||
n_p.param("FootContactsEnabled", foot_contacts_enabled_, true);
|
||||
|
||||
node_handle_.param("Nao_PublishIMU", imu_published_, true);
|
||||
n_p.param("PublishIMU", imu_published_, true);
|
||||
|
||||
node_handle_.param("Nao_TopicQueue", topic_queue_, 50);
|
||||
n_p.param("TopicQueue", topic_queue_, 50);
|
||||
|
||||
node_handle_.param("Nao_Prefix", prefix_, string("nao_dcm"));
|
||||
n_p.param("Prefix", prefix_, string("nao_dcm"));
|
||||
prefix_ = prefix_+"/";
|
||||
|
||||
node_handle_.param("Nao_LowCommunicationFrequency", low_freq_, 10.0);
|
||||
node_handle_.param("Nao_HighCommunicationFrequency", high_freq_, 100.0);
|
||||
node_handle_.param("Nao_ControllerFrequency", controller_freq_, 15.0);
|
||||
n_p.param("LowCommunicationFrequency", low_freq_, 10.0);
|
||||
n_p.param("HighCommunicationFrequency", high_freq_, 100.0);
|
||||
n_p.param("ControllerFrequency", controller_freq_, 15.0);
|
||||
}
|
||||
|
||||
void Nao::brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier)
|
||||
|
||||
@ -27,6 +27,7 @@ int main( int argc, char** argv )
|
||||
string pip = "127.0.0.1";
|
||||
ros::init(argc, argv, "nao_dcm_driver");
|
||||
ros::NodeHandle n;
|
||||
ros::NodeHandle n_p("~");
|
||||
if(!ros::master::check())
|
||||
{
|
||||
cerr<<"Could not contact master!\nQuitting... "<<endl;
|
||||
@ -40,16 +41,20 @@ int main( int argc, char** argv )
|
||||
string broker_ip = "0.0.0.0";
|
||||
|
||||
// Load Params from Parameter Server
|
||||
n.param("Nao_RobotIP", pip, string("127.0.0.1"));
|
||||
n.param("Nao_RobotPort", pport,9559);
|
||||
n.param("Nao_DriverBrokerPort", broker_port, 54000);
|
||||
n.param("Nao_DriverBrokerIP", broker_ip, string("0.0.0.0"));
|
||||
n_p.param("RobotIP", pip, string("127.0.0.1"));
|
||||
n_p.param("RobotPort", pport,9559);
|
||||
n_p.param("DriverBrokerPort", broker_port, 54000);
|
||||
n_p.param("DriverBrokerIP", broker_ip, string("0.0.0.0"));
|
||||
|
||||
// Create your own broker
|
||||
boost::shared_ptr<AL::ALBroker> broker;
|
||||
try
|
||||
{
|
||||
broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0);
|
||||
|
||||
// Workaround because stiffness does not work well via DCM
|
||||
AL::ALProxy tempMotion("ALMotion", pip, pport);
|
||||
tempMotion.callVoid("setStiffnesses","Body",1.0f);
|
||||
}
|
||||
catch(...)
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user