diff --git a/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp b/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp index f8ae6cb..6c4363f 100644 --- a/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp +++ b/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp @@ -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 broker; diff --git a/nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml b/nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml index fde1071..a26550c 100644 --- a/nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml +++ b/nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml @@ -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 \ No newline at end of file +Prefix: nao_dcm \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup.launch b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup.launch index 56df3e1..1768576 100644 --- a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup.launch +++ b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup.launch @@ -3,56 +3,54 @@ - + + + + + + + + + + + + + + + + - - - - - - - + + - - + + - - + + - - - - - - - - - - - - - + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup_remote.launch b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup_remote.launch index c94a997..8a29d51 100644 --- a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup_remote.launch +++ b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup_remote.launch @@ -3,33 +3,35 @@ - + + + + + + + + + + + + + + + + - - - - - - - + + - - - - - - - - - - - - - - - - - + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup.launch b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup.launch index 602fe7e..9bb3f25 100644 --- a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup.launch +++ b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup.launch @@ -1,58 +1,56 @@ - + - - + + + + - - - - + + - - + + - - + + + + + + + + + - - + + - - - - - + + - - - - + + + + + + + + + + + + + + --> \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup_remote.launch b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup_remote.launch index d49b861..ce6f63a 100644 --- a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup_remote.launch +++ b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup_remote.launch @@ -3,33 +3,35 @@ - - + + + + - - - - + + - - + + - - + + + + + + + + + - - - - - - - - - - - - - - + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_driver/src/nao.cpp b/nao_dcm_robot/nao_dcm_driver/src/nao.cpp index eb57fca..31da861 100644 --- a/nao_dcm_robot/nao_dcm_driver/src/nao.cpp +++ b/nao_dcm_robot/nao_dcm_driver/src/nao.cpp @@ -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 stiff = vector(number_of_joints_,1.0f); - vector times = vector(number_of_joints_,0); + vector stiff = vector(number_of_joints_-1,1.0f); + vector times = vector(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) diff --git a/nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp b/nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp index 2da7150..206d78f 100644 --- a/nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp +++ b/nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp @@ -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... "< 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(...) {